/**
 * Copyright (c) 2012-2014, Yang Yang <cntototo@gmail.com>
 *
 * This software may be distributed under the terms of the New BSD License.
 * See README for more details.
 */

#include "driver.h"

#define LOG_TAG "DRIVER-NL80211"

#include <netlink/genl/genl.h>
#include <netlink/genl/family.h>
#include <netlink/genl/ctrl.h>
#include <netlink/msg.h>
#include <netlink/attr.h>
#include <netlink/netlink.h>
#include <net/if.h>
#include <linux/genetlink.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>

#include "include.h"
#include "def.h"
#include "util.h"
#include "log.h"
#include "if_manager.h"
#include "ieee802_11_defs.h"
#include "loop.h"
#include "nl80211_copy.h"
#include "linux_spec.h"
#include "event.h"

#ifndef SO_WIFI_STATUS
#define SO_WIFI_STATUS 41
#define SCM_WIFI_STATUS	SO_WIFI_STATUS
#endif

#ifndef SO_EE_ORIGIN_TXSTATUS
#define SO_EE_ORIGIN_TXSTATUS 4
#endif

#ifndef PACKET_TX_TIMESTAMP
#define PACKET_TX_TIMESTAMP 16
#endif

#define MAX_WIPHY_NAME_LEN 32

/**
 * struct nl80211_driver_status -- Driver run-time status for NL80211
 */
struct nl80211_driver_status {
	/** Interface type */
	enum nl80211_iftype nl_iftype;
	/** Hardware info status */
	struct hw_info_status {
		/** Hardware info array */
		struct wfs_driver_hw_info *hw_info;
		/** Number of elems in array */
		u32 hw_info_num;
	} hw;
	/** BSSID */
	u8 bssid[ETH_ALEN];
	/** SSID */
	u8 ssid[MAX_SSID_LEN];
	/** Length of SSID */
	u8 ssid_len;
	/** Is interface already up before init */
	u8 is_iface_already_up;
	/** Frequency in MHz */
	u32 freq;
	/** Status if this device is AP */
	struct ap_status {
		/** Is beacon set or not */
		u8 is_beacon_set;
		/** EAPOL TX sock */
		int eapol_tx_sock;
	} ap;
	/** Status if this device is STA */
	struct sta_status {
		/** Is associated or not */
		u8 is_associated;
		/** Authentication BSSID */
		u8 auth_bssid[ETH_ALEN];
		/** Attempted authentication BSSID */
		u8 auth_attempt_bssid[ETH_ALEN];
		/** Previous BSSID */
		u8 prev_bssid[ETH_ALEN];
		/** Is authentication retry needed */
		u8 retry_auth;
	} sta;
	/** Scan status */
	struct scan_status {
		/** Current scan state */
		enum scan_states {
			NO_SCAN, SCAN_REQUESTED, SCAN_STARTED, SCAN_COMPLETED,
			SCAN_ABORTED, SCHED_SCAN_REQUESTED, SCHED_SCAN_STARTED,
			SCHED_SCAN_STOPPED, SCHED_SCAN_RESULTS
		} scan_state;
		/** Devices to scan */
		struct match_device *match_devs;
		/** Number of devices to scan */
		u32 match_devs_num;
	} scan;
	/** Remain on channel status */
	struct roc_status {
		/** Remain on channel cookie */
		u64 cookie;
		/** Is there pending remain on channel request or not */
		u8 is_pending;
	} roc;
	/** Frame cookie */
	u64 frame_cookie;
};

/**
 * struct nl80211_driver_info -- Driver information for NL80211
 */
struct nl80211_driver_info {
	struct nl_cb *nl_cb;
	struct nl_handle *cmd_handle;
	struct nl_handle *evt_handle;
	/** Generic netlink family id */
	s32 family_id;
	/** MAC address */
	u8 addr[ETH_ALEN];
	/** WIPHY name */
	char wiphy_name[MAX_WIPHY_NAME_LEN];
	/** Need monitor interface */
	u8 need_monitor;
	/** IOCTL socket */
	int ioctl_sock;
	/** Current driver status */
	struct nl80211_driver_status cur_status;
	/** Driver capability */
	struct wfs_driver_capa capa;
};


/**
 * struct nl80211_bss_info -- BSS info
 */
struct nl80211_bss_info {
	struct nl80211_driver_info *info;
	struct wfs_driver_scan_results *scan_results;
	u32 assoc_freq;
	u8 assoc_bssid[ETH_ALEN];
};


/**
 * struct nl80211_get_params -- Params got from NL80211
 */
struct nl80211_get_params {
	u32 wiphy_idx;
	enum nl80211_iftype nl_iftype;
	u8 *addr;
};


/**
 * struct nl80211_wiphy_info -- WIPHY info
 */
struct nl80211_wiphy_info {
	struct wfs_driver_hw_info *hw_info_array;
	u32 hw_info_num;
	s32 last_nla_type;
	u32 last_chan_idx;
};


/**
 * struct nl80211_mcast_group_info -- Multicast group info
 */
struct nl80211_mcast_group_info {
	const char *group;
	int id;
};

/***********************************************************
 * Forward declarations
 ***********************************************************
 */
static enum wfs_internal_err
nl_get_assoc_freq(struct wfs_iface *cur_if, u32 *freq);

static int send_and_recv(struct wfs_iface *cur_if, struct nl_msg *msg,
		int(*valid_handler)(struct nl_msg *, void *), void *valid_arg);

/***********************************************************
 * Local functions
 ***********************************************************
 */

#ifdef CONFIG_LIBNL20
/* libnl 2.0 compatibility code */
#define nl_handle nl_sock
#define nl80211_handle_alloc nl_socket_alloc_cb
#define nl80211_handle_destroy nl_socket_free
#else
/*
 * libnl 1.1 has a bug, it tries to allocate socket numbers densely
 * but when you free a socket again it will mess up its bitmap and
 * and use the wrong number the next time it needs a socket ID.
 * Therefore, we wrap the handle alloc/destroy and add our own pid
 * accounting.
 */
static u32 port_bitmap[32] = { 0 };

static struct nl_handle *nl80211_handle_alloc(void *cb)
{
	struct nl_handle *handle;
	u32 pid = getpid() & 0x3FFFFF;
	int i;

	handle = nl_handle_alloc_cb(cb);

	for (i = 0; i < 1024; i++) {
		if (port_bitmap[i / 32] & (1 << (i % 32)))
			continue;
		port_bitmap[i / 32] |= 1 << (i % 32);
		pid += i << 22;
		break;
	}

	nl_socket_set_local_port(handle, pid);

	return handle;
}


static void nl80211_handle_destroy(struct nl_handle *handle)
{
	u32 port = nl_socket_get_local_port(handle);

	port >>= 22;
	port_bitmap[port / 32] &= ~(1 << (port % 32));

	nl_handle_destroy(handle);
}
#endif /* CONFIG_LIBNL20 */


static void *
add_genl_header(struct nl_msg *msg, s32 family_id, s32 flags, u8 cmd)
{
	assert(msg);

	return genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, family_id,
			0, flags, cmd, 0);
}


static struct nl_handle *
create_handle(struct nl_cb *cb, const char *dbg)
{
	struct nl_handle *handle = NULL;

	assert(cb && dbg);

	handle = nl80211_handle_alloc(cb);
	if (!handle) {
		wfs_printf(LOG_ERR, "Failed to allocate netlink "
				"callbacks (%s)", dbg);
		return NULL;
	}

	if (genl_connect(handle)) {
		wfs_printf(LOG_ERR, "Failed to connect to generic "
				"netlink (%s)", dbg);
		nl80211_handle_destroy(handle);
		return NULL;
	}

	return handle;
}


static int no_seq_check(struct nl_msg *msg, void *arg)
{
	return NL_OK;
}


static const char *nl80211_cmd_to_str(enum nl80211_commands cmd)
{
#define C2S(x) case x: return #x;
	switch (cmd) {
	C2S(NL80211_CMD_UNSPEC)

	C2S(NL80211_CMD_GET_WIPHY)
	C2S(NL80211_CMD_SET_WIPHY)
	C2S(NL80211_CMD_NEW_WIPHY)
	C2S(NL80211_CMD_DEL_WIPHY)

	C2S(NL80211_CMD_GET_INTERFACE)
	C2S(NL80211_CMD_SET_INTERFACE)
	C2S(NL80211_CMD_NEW_INTERFACE)
	C2S(NL80211_CMD_DEL_INTERFACE)

	C2S(NL80211_CMD_GET_KEY)
	C2S(NL80211_CMD_SET_KEY)
	C2S(NL80211_CMD_NEW_KEY)
	C2S(NL80211_CMD_DEL_KEY)

	C2S(NL80211_CMD_GET_BEACON)
	C2S(NL80211_CMD_SET_BEACON)
	C2S(NL80211_CMD_START_AP)
	C2S(NL80211_CMD_STOP_AP)

	C2S(NL80211_CMD_GET_STATION)
	C2S(NL80211_CMD_SET_STATION)
	C2S(NL80211_CMD_NEW_STATION)
	C2S(NL80211_CMD_DEL_STATION)

	C2S(NL80211_CMD_GET_MPATH)
	C2S(NL80211_CMD_SET_MPATH)
	C2S(NL80211_CMD_NEW_MPATH)
	C2S(NL80211_CMD_DEL_MPATH)

	C2S(NL80211_CMD_SET_BSS)

	C2S(NL80211_CMD_SET_REG)
	C2S(NL80211_CMD_REQ_SET_REG)

	C2S(NL80211_CMD_GET_MESH_CONFIG)
	C2S(NL80211_CMD_SET_MESH_CONFIG)

	C2S(NL80211_CMD_SET_MGMT_EXTRA_IE)

	C2S(NL80211_CMD_GET_REG)

	C2S(NL80211_CMD_GET_SCAN)
	C2S(NL80211_CMD_TRIGGER_SCAN)
	C2S(NL80211_CMD_NEW_SCAN_RESULTS)
	C2S(NL80211_CMD_SCAN_ABORTED)

	C2S(NL80211_CMD_REG_CHANGE)

	C2S(NL80211_CMD_AUTHENTICATE)
	C2S(NL80211_CMD_ASSOCIATE)
	C2S(NL80211_CMD_DEAUTHENTICATE)
	C2S(NL80211_CMD_DISASSOCIATE)

	C2S(NL80211_CMD_MICHAEL_MIC_FAILURE)

	C2S(NL80211_CMD_REG_BEACON_HINT)

	C2S(NL80211_CMD_JOIN_IBSS)
	C2S(NL80211_CMD_LEAVE_IBSS)

	C2S(NL80211_CMD_TESTMODE)

	C2S(NL80211_CMD_CONNECT)
	C2S(NL80211_CMD_ROAM)
	C2S(NL80211_CMD_DISCONNECT)

	C2S(NL80211_CMD_SET_WIPHY_NETNS)

	C2S(NL80211_CMD_GET_SURVEY)
	C2S(NL80211_CMD_NEW_SURVEY_RESULTS)

	C2S(NL80211_CMD_SET_PMKSA)
	C2S(NL80211_CMD_DEL_PMKSA)
	C2S(NL80211_CMD_FLUSH_PMKSA)

	C2S(NL80211_CMD_REMAIN_ON_CHANNEL)
	C2S(NL80211_CMD_CANCEL_REMAIN_ON_CHANNEL)

	C2S(NL80211_CMD_SET_TX_BITRATE_MASK)

	C2S(NL80211_CMD_REGISTER_FRAME)
	C2S(NL80211_CMD_FRAME)
	C2S(NL80211_CMD_FRAME_TX_STATUS)

	C2S(NL80211_CMD_SET_POWER_SAVE)
	C2S(NL80211_CMD_GET_POWER_SAVE)

	C2S(NL80211_CMD_SET_CQM)
	C2S(NL80211_CMD_NOTIFY_CQM)

	C2S(NL80211_CMD_SET_CHANNEL)
	C2S(NL80211_CMD_SET_WDS_PEER)

	C2S(NL80211_CMD_FRAME_WAIT_CANCEL)

	C2S(NL80211_CMD_JOIN_MESH)
	C2S(NL80211_CMD_LEAVE_MESH)

	C2S(NL80211_CMD_UNPROT_DEAUTHENTICATE)
	C2S(NL80211_CMD_UNPROT_DISASSOCIATE)

	C2S(NL80211_CMD_NEW_PEER_CANDIDATE)

	C2S(NL80211_CMD_GET_WOWLAN)
	C2S(NL80211_CMD_SET_WOWLAN)

	C2S(NL80211_CMD_START_SCHED_SCAN)
	C2S(NL80211_CMD_STOP_SCHED_SCAN)
	C2S(NL80211_CMD_SCHED_SCAN_RESULTS)
	C2S(NL80211_CMD_SCHED_SCAN_STOPPED)

	C2S(NL80211_CMD_SET_REKEY_OFFLOAD)

	C2S(NL80211_CMD_PMKSA_CANDIDATE)

	C2S(NL80211_CMD_TDLS_OPER)
	C2S(NL80211_CMD_TDLS_MGMT)

	C2S(NL80211_CMD_UNEXPECTED_FRAME)

	C2S(NL80211_CMD_PROBE_CLIENT)

	C2S(NL80211_CMD_REGISTER_BEACONS)

	C2S(NL80211_CMD_UNEXPECTED_4ADDR_FRAME)

	C2S(NL80211_CMD_SET_NOACK_MAP)

	C2S(NL80211_CMD_CH_SWITCH_NOTIFY)

	C2S(NL80211_CMD_START_P2P_DEVICE)
	C2S(NL80211_CMD_STOP_P2P_DEVICE)

	C2S(NL80211_CMD_CONN_FAILED)

	C2S(NL80211_CMD_SET_MCAST_RATE)

	C2S(NL80211_CMD_SET_MAC_ACL)

	C2S(NL80211_CMD_RADAR_DETECT)

	C2S(NL80211_CMD_GET_PROTOCOL_FEATURES)

	C2S(NL80211_CMD_UPDATE_FT_IES)
	C2S(NL80211_CMD_FT_EVENT)

	C2S(NL80211_CMD_CRIT_PROTOCOL_START)
	C2S(NL80211_CMD_CRIT_PROTOCOL_STOP)

	C2S(NL80211_CMD_GET_COALESCE)
	C2S(NL80211_CMD_SET_COALESCE)

	C2S(NL80211_CMD_CHANNEL_SWITCH)

	C2S(NL80211_CMD_VENDOR)

	C2S(NL80211_CMD_SET_QOS_MAP)

	default:
		return "NL80211_CMD_UNKNOWN";
	}
#undef C2S
}


static void nl_send_mlme_timeout_event(struct wfs_iface *cur_if,
		enum nl80211_commands cmd, struct nlattr *addr)
{
	union wfs_event_data event;
	enum wfs_event_type type;

	memset(&event, 0, sizeof(event));

	if (nla_len(addr) != ETH_ALEN)
		return;

	wfs_printf(LOG_DEBUG, "Driver MLME event %d (%s) timeout with "
			MACSTR, cmd, nl80211_cmd_to_str(cmd), nla_data(addr));

	if (cmd == NL80211_CMD_AUTHENTICATE)
		type = EVT_AUTH_TIMEOUT;
	else if (cmd == NL80211_CMD_ASSOCIATE)
		type = EVT_ASSOC_TIMEOUT;
	else
		return;

	memcpy(event.timeout.addr, nla_data(addr), ETH_ALEN);
	wfs_process_event(cur_if, type, &event);
}


static void nl_send_mlme_assoc_event(struct wfs_iface *cur_if, const u8 *frame,
		u32 len)
{
	struct nl80211_driver_info *info = NULL;
	const struct ieee80211_mgmt *mgmt = NULL;
	union wfs_event_data event;
	u16 status_code = 0;

	assert(cur_if && cur_if->driver_info && frame);

	info = cur_if->driver_info;
	mgmt = (const struct ieee80211_mgmt *) frame;
	memset(&event, 0, sizeof(event));

	wfs_printf(LOG_DEBUG, "Driver MLME association event");

	if (len < IEEE80211_HDRLEN + sizeof(mgmt->u.auth)) {
		wfs_printf(LOG_DEBUG, "Too short association event frame");
		return;
	}

	status_code = le_to_host16(mgmt->u.assoc_resp.status_code);
	if (status_code != WLAN_STATUS_SUCCESS) {
		memcpy(event.assoc_reject.bssid, mgmt->bssid, ETH_ALEN);
		if (len > IEEE80211_HDRLEN + sizeof(mgmt->u.assoc_resp)) {
			event.assoc_reject.resp_ie =
				(u8 *) mgmt->u.assoc_resp.variable;
			event.assoc_reject.resp_ie_len = len - IEEE80211_HDRLEN
					- sizeof(mgmt->u.assoc_resp);
		}
		event.assoc_reject.status_code = status_code;
		wfs_process_event(cur_if, EVT_ASSOC_REJECT, &event);
		return;
	}

	info->cur_status.sta.is_associated = 1;
	memcpy(info->cur_status.bssid, mgmt->sa, ETH_ALEN);
	memcpy(info->cur_status.sta.prev_bssid, mgmt->sa, ETH_ALEN);

	if (len > IEEE80211_HDRLEN + sizeof(mgmt->u.assoc_resp)) {
		event.assoc.resp_ie = (u8 *) mgmt->u.assoc_resp.variable;
		event.assoc.resp_ie_len =
			len - IEEE80211_HDRLEN - sizeof(mgmt->u.assoc_resp);
	}

	event.assoc.freq = info->cur_status.freq;

	wfs_process_event(cur_if, EVT_ASSOC, &event);
}


static void nl_send_mlme_auth_event(struct wfs_iface *cur_if, const u8 *frame,
		u32 len)
{
	struct nl80211_driver_info *info = NULL;
	const struct ieee80211_mgmt *mgmt = NULL;
	union wfs_event_data event;

	assert(cur_if && cur_if->driver_info && frame);

	info = cur_if->driver_info;
	mgmt = (const struct ieee80211_mgmt *) frame;
	memset(&event, 0, sizeof(event));

	wfs_printf(LOG_DEBUG, "Driver MLME authentication event");

	if (len < IEEE80211_HDRLEN + sizeof(mgmt->u.auth)) {
		wfs_printf(LOG_DEBUG, "Too short authentication event frame");
		return;
	}

	memcpy(info->cur_status.sta.auth_bssid, mgmt->sa, ETH_ALEN);
	memset(info->cur_status.sta.auth_attempt_bssid, 0, ETH_ALEN);
	memcpy(event.auth.peer, mgmt->sa, ETH_ALEN);
	event.auth.auth_type = le_to_host16(mgmt->u.auth.auth_alg);
	event.auth.auth_transaction =
		le_to_host16(mgmt->u.auth.auth_transaction);
	event.auth.status_code = le_to_host16(mgmt->u.auth.status_code);
	if (len > IEEE80211_HDRLEN + sizeof(mgmt->u.auth)) {
		event.auth.ie = mgmt->u.auth.variable;
		event.auth.ie_len =
			len - IEEE80211_HDRLEN - sizeof(mgmt->u.auth);
	}

	wfs_process_event(cur_if, EVT_AUTH, &event);
}


static int is_deauth_disassoc_from_unknown(struct wfs_iface *cur_if,
		const struct ieee80211_mgmt *mgmt)
{
	struct nl80211_driver_info *info = NULL;
	const u8 *bssid = NULL;

	assert(cur_if && cur_if->driver_info && mgmt);

	info = cur_if->driver_info;
	bssid = mgmt->bssid;

	if ((info->capa.flags & DRIVER_CAPA_FLAGS_SME)
		&& !info->cur_status.sta.is_associated
		&& !is_same_mac(bssid, info->cur_status.sta.auth_bssid)
		&& !is_same_mac(bssid, info->cur_status.sta.auth_attempt_bssid)
		&& is_same_mac(bssid, info->cur_status.sta.prev_bssid)) {
		wfs_printf(LOG_DEBUG, "Ignore deauth/disassoc event from old AP"
			" "MACSTR" when already authenticating with "MACSTR,
			MAC2STR(bssid),
			MAC2STR(info->cur_status.sta.auth_attempt_bssid));
		return TRUE;
	}

	if (info->cur_status.sta.is_associated
		&& !is_same_mac(bssid, info->cur_status.bssid)
		&& !is_same_mac(bssid, info->cur_status.sta.auth_bssid)) {
		wfs_printf(LOG_DEBUG, "Ignore deauth/disassoc event from "
			"unknown BSSID "MACSTR, MAC2STR(bssid));
		return TRUE;
	}

	return FALSE;
}


static void nl_mark_disconnected(struct wfs_iface *cur_if)
{
	struct nl80211_driver_info *info = NULL;

	assert(cur_if && cur_if->driver_info);

	info = cur_if->driver_info;

	if (info->cur_status.sta.is_associated)
		memcpy(info->cur_status.sta.prev_bssid, info->cur_status.bssid,
				ETH_ALEN);
	info->cur_status.sta.is_associated = 0;
	memset(info->cur_status.bssid, 0, ETH_ALEN);
}


static void nl_send_mlme_deauth_event(struct wfs_iface *cur_if, const u8 *frame,
		u32 len)
{
	struct nl80211_driver_info *info = NULL;
	const struct ieee80211_mgmt *mgmt = NULL;
	union wfs_event_data event;

	assert(cur_if && cur_if->driver_info && frame);

	info = cur_if->driver_info;
	mgmt = (const struct ieee80211_mgmt *) frame;
	memset(&event, 0, sizeof(event));

	wfs_printf(LOG_DEBUG, "Driver MLME deauthentication event");

	if (len < IEEE80211_HDRLEN || (len >= IEEE80211_HDRLEN
			&& is_deauth_disassoc_from_unknown(cur_if, mgmt)))
		return;

	nl_mark_disconnected(cur_if);

	event.deauth.local = is_same_mac(mgmt->sa, info->addr);
	memcpy(event.deauth.addr, mgmt->bssid, ETH_ALEN);
	if (len >= IEEE80211_HDRLEN + sizeof(mgmt->u.deauth))
		event.deauth.reason_code =
				le_to_host16(mgmt->u.deauth.reason_code);
	if (len > IEEE80211_HDRLEN + sizeof(mgmt->u.deauth)) {
		event.deauth.ie = mgmt->u.deauth.variable;
		event.deauth.ie_len =
			len - IEEE80211_HDRLEN - sizeof(mgmt->u.deauth);
	}

	wfs_process_event(cur_if, EVT_DEAUTH, &event);
}


static void nl_send_mlme_disassoc_event(struct wfs_iface *cur_if,
		const u8 *frame, u32 len)
{
	struct nl80211_driver_info *info = NULL;
	const struct ieee80211_mgmt *mgmt = NULL;
	union wfs_event_data event;

	assert(cur_if && cur_if->driver_info && frame);

	info = cur_if->driver_info;
	mgmt = (const struct ieee80211_mgmt *) frame;
	memset(&event, 0, sizeof(event));

	wfs_printf(LOG_DEBUG, "Driver MLME disassociation event");

	if (len < IEEE80211_HDRLEN || (len >= IEEE80211_HDRLEN
			&& is_deauth_disassoc_from_unknown(cur_if, mgmt)))
		return;

	nl_mark_disconnected(cur_if);

	event.disassoc.local = is_same_mac(mgmt->sa, info->addr);
	memcpy(event.disassoc.addr, mgmt->bssid, ETH_ALEN);
	if (len >= IEEE80211_HDRLEN + sizeof(mgmt->u.disassoc))
		event.disassoc.reason_code =
				le_to_host16(mgmt->u.disassoc.reason_code);
	if (len > IEEE80211_HDRLEN + sizeof(mgmt->u.disassoc)) {
		event.disassoc.ie = mgmt->u.disassoc.variable;
		event.disassoc.ie_len =
			len - IEEE80211_HDRLEN - sizeof(mgmt->u.disassoc);
	}

	wfs_process_event(cur_if, EVT_DISASSOC, &event);
}


static void nl_send_mlme_rx_mgmt_event(struct wfs_iface *cur_if,
		const u8 *frame, u32 len, struct nlattr *freq,
		struct nlattr *signal)
{
	const struct ieee80211_mgmt *mgmt = NULL;
	union wfs_event_data event;
	u16 fc = 0, stype = 0;

	assert(cur_if && frame);

	mgmt = (const struct ieee80211_mgmt *) frame;
	memset(&event, 0, sizeof(event));

	wfs_printf(LOG_DEBUG, "Driver MLME frame event");

	if (len < IEEE80211_HDRLEN) {
		wfs_printf(LOG_DEBUG, "Too short frame event frame");
		return;
	}

	event.rx_mgmt.frame = frame;
	event.rx_mgmt.frame_len = len;
	if (freq)
		event.rx_mgmt.freq = nla_get_u32(freq);
	if (signal)
		event.rx_mgmt.signal = nla_get_u32(signal);

	fc = le_to_host16(mgmt->frame_control);
	stype = WLAN_FC_GET_STYPE(fc);
	wfs_printf(LOG_DEBUG, "RX frame freq=%u signal=%u stype=%u len=%u",
			event.rx_mgmt.freq, event.rx_mgmt.signal, stype, len);

	wfs_process_event(cur_if, EVT_RX_MGMT, &event);
}


static int is_ap_iftype(enum nl80211_iftype iftype)
{
	return (iftype == NL80211_IFTYPE_AP ||
		iftype == NL80211_IFTYPE_P2P_GO);
}


static void nl_send_mlme_tx_status_event(struct wfs_iface *cur_if,
		const u8 *frame, u32 len, struct nlattr *cookie,
		struct nlattr *ack)
{
	struct nl80211_driver_info *info = NULL;
	const struct ieee80211_mgmt *mgmt = NULL;
	union wfs_event_data event;
	u16 fc = 0;

	assert(cur_if && cur_if->driver_info && frame);

	info = cur_if->driver_info;
	mgmt = (const struct ieee80211_mgmt *) frame;
	memset(&event, 0, sizeof(event));

	wfs_printf(LOG_DEBUG, "Driver MLME frame TX status event");

	if (len < IEEE80211_HDRLEN) {
		wfs_printf(LOG_DEBUG, "Too short frame TX status event frame");
		return;
	}

	if (!is_ap_iftype(info->cur_status.nl_iftype)) {
		u64 cookie_val = 0;

		if (!cookie)
			return;
		cookie_val = nla_get_u64(cookie);
		wfs_printf(LOG_DEBUG, "Frame TX status: cookie=0%llx%s "
				"(ack=%u)", (long long unsigned int) cookie_val,
				cookie_val == info->cur_status.frame_cookie ?
				" (match)" : " (unknown)", ack != NULL);
		if (cookie_val != info->cur_status.frame_cookie)
			return;
	}

	fc = le_to_host16(mgmt->frame_control);
	event.tx_status.type = WLAN_FC_GET_TYPE(fc);
	event.tx_status.stype = WLAN_FC_GET_STYPE(fc);
	memcpy(event.tx_status.dst, mgmt->da, ETH_ALEN);
	event.tx_status.frame = frame;
	event.tx_status.frame_len = len;
	event.tx_status.ack = ack != NULL;

	wfs_process_event(cur_if, EVT_TX_STATUS, &event);
}


static void nl_send_mlme_unprot_deauth_event(struct wfs_iface *cur_if,
		const u8 *frame, u32 len)
{
	const struct ieee80211_mgmt *mgmt = NULL;
	union wfs_event_data event;

	assert(cur_if && frame);

	mgmt = (const struct ieee80211_mgmt *) frame;
	memset(&event, 0, sizeof(event));

	wfs_printf(LOG_DEBUG, "Driver MLME Unprot deauthentication event");

	if (len < IEEE80211_HDRLEN) {
		wfs_printf(LOG_DEBUG, "Too short unprot deauth event frame");
		return;
	}

	memcpy(event.unprot_deauth.sa, mgmt->sa, ETH_ALEN);
	memcpy(event.unprot_deauth.da, mgmt->da, ETH_ALEN);
	if (len >= IEEE80211_HDRLEN + sizeof(mgmt->u.deauth))
		event.unprot_deauth.reason_code =
				le_to_host16(mgmt->u.deauth.reason_code);

	wfs_process_event(cur_if, EVT_UNPROT_DEAUTH, &event);
}


static void nl_send_mlme_unprot_disassoc_event(struct wfs_iface *cur_if,
		const u8 *frame, u32 len)
{
	const struct ieee80211_mgmt *mgmt = NULL;
	union wfs_event_data event;

	assert(cur_if && frame);

	mgmt = (const struct ieee80211_mgmt *) frame;
	memset(&event, 0, sizeof(event));

	wfs_printf(LOG_DEBUG, "Driver MLME Unprot disassociation event");

	if (len < IEEE80211_HDRLEN) {
		wfs_printf(LOG_DEBUG, "Too short unprot disassoc event frame");
		return;
	}

	memcpy(event.unprot_disassoc.sa, mgmt->sa, ETH_ALEN);
	memcpy(event.unprot_disassoc.da, mgmt->da, ETH_ALEN);
	if (len >= IEEE80211_HDRLEN + sizeof(mgmt->u.disassoc))
		event.unprot_disassoc.reason_code =
				le_to_host16(mgmt->u.disassoc.reason_code);

	wfs_process_event(cur_if, EVT_UNPROT_DISASSOC, &event);
}


static void nl_send_mlme_event(struct wfs_iface *cur_if,
		enum nl80211_commands cmd, struct nlattr *tb[])
{
	struct nl80211_driver_info *info = NULL;
	struct nlattr *frame = NULL, *addr = NULL, *timed_out = NULL;
	struct nlattr *freq = NULL, *ack = NULL, *cookie = NULL, *signal = NULL;
	const u8 *frame_data = NULL;
	u32 frame_len = 0;

	assert(cur_if && cur_if->driver_info && tb);

	info = cur_if->driver_info;
	frame = tb[NL80211_ATTR_FRAME];
	addr = tb[NL80211_ATTR_MAC];
	timed_out = tb[NL80211_ATTR_TIMED_OUT];
	freq = tb[NL80211_ATTR_WIPHY_FREQ];
	ack = tb[NL80211_ATTR_ACK];
	cookie = tb[NL80211_ATTR_COOKIE];
	signal = tb[NL80211_ATTR_RX_SIGNAL_DBM];

	if (timed_out && addr) {
		nl_send_mlme_timeout_event(cur_if, cmd, addr);
		return;
	}

	if (!frame) {
		wfs_printf(LOG_DEBUG, "Driver MLME event %d (%s) without frame "
				"data", cmd, nl80211_cmd_to_str(cmd));
		return;
	}

	frame_data = nla_data(frame);
	frame_len = nla_len(frame);
	if (frame_len < 4 + 2 * ETH_ALEN) {
		wfs_printf(LOG_DEBUG, "Driver MLME event %d (%s) too short",
				cmd, nl80211_cmd_to_str(cmd));
		return;
	}

	if (cmd != NL80211_CMD_FRAME_TX_STATUS && !(frame_data[4] & 0x01)
		&& !is_same_mac(info->addr, frame_data + 4)
		&& !is_same_mac(info->addr, frame_data + 4 + ETH_ALEN)) {
		wfs_printf(LOG_DEBUG, "Ignore driver MLME event for unknown "
				"address");
		return;
	}

	wfs_hexdump(LOG_DEBUG, "Driver MLME event frame", nla_data(frame),
			nla_len(frame));

	switch (cmd) {
	case NL80211_CMD_ASSOCIATE:
		nl_send_mlme_assoc_event(cur_if, nla_data(frame),
				nla_len(frame));
		break;
	case NL80211_CMD_AUTHENTICATE:
		nl_send_mlme_auth_event(cur_if, nla_data(frame),
				nla_len(frame));
		break;
	case NL80211_CMD_DEAUTHENTICATE:
		nl_send_mlme_deauth_event(cur_if, nla_data(frame),
				nla_len(frame));
		break;
	case NL80211_CMD_DISASSOCIATE:
		nl_send_mlme_disassoc_event(cur_if, nla_data(frame),
				nla_len(frame));
		break;
	case NL80211_CMD_FRAME:
		nl_send_mlme_rx_mgmt_event(cur_if, nla_data(frame),
				nla_len(frame), freq, signal);
		break;
	case NL80211_CMD_FRAME_TX_STATUS:
		nl_send_mlme_tx_status_event(cur_if, nla_data(frame),
				nla_len(frame), cookie, ack);
		break;
	case NL80211_CMD_UNPROT_DEAUTHENTICATE:
		nl_send_mlme_unprot_deauth_event(cur_if, nla_data(frame),
				nla_len(frame));
		break;
	case NL80211_CMD_UNPROT_DISASSOCIATE:
		nl_send_mlme_unprot_disassoc_event(cur_if, nla_data(frame),
				nla_len(frame));
		break;
	default:
		break;
	}
}


static void nl_send_connect_event(struct wfs_iface *cur_if,
		enum nl80211_commands cmd, struct nlattr *tb[])
{
	struct nl80211_driver_info *info = NULL;
	struct nlattr *status = NULL, *addr = NULL, *req_ie = NULL;
	struct nlattr *resp_ie = NULL;
	union wfs_event_data event;

	assert(cur_if && cur_if->driver_info && tb);

	info = cur_if->driver_info;
	status = tb[NL80211_ATTR_STATUS_CODE];
	addr = tb[NL80211_ATTR_MAC];
	req_ie = tb[NL80211_ATTR_REQ_IE];
	resp_ie = tb[NL80211_ATTR_RESP_IE];
	memset(&event, 0, sizeof(event));

	if (info->capa.flags & DRIVER_CAPA_FLAGS_SME) {
		wfs_printf(LOG_DEBUG, "Ignore connect event %d (%s) when using "
				"userspace SME", cmd, nl80211_cmd_to_str(cmd));
		return;
	}

	if (cmd == NL80211_CMD_CONNECT
		&& (!status || nla_get_u16(status) != WLAN_STATUS_SUCCESS)) {
		if (addr)
			memcpy(event.assoc_reject.bssid, nla_data(addr),
					ETH_ALEN);
		if (resp_ie) {
			event.assoc_reject.resp_ie = nla_data(resp_ie);
			event.assoc_reject.resp_ie_len = nla_len(resp_ie);
		}
		if (status)
			event.assoc_reject.status_code = nla_get_u16(status);
		wfs_process_event(cur_if, EVT_ASSOC_REJECT, &event);
		return;
	}

	info->cur_status.sta.is_associated = 1;
	if (addr) {
		memcpy(info->cur_status.bssid, nla_data(addr), ETH_ALEN);
		memcpy(info->cur_status.sta.prev_bssid, nla_data(addr),
				ETH_ALEN);
	}
	if (req_ie) {
		event.assoc.req_ie = nla_data(req_ie);
		event.assoc.req_ie_len = nla_len(req_ie);
	}
	if (resp_ie) {
		event.assoc.resp_ie = nla_data(resp_ie);
		event.assoc.resp_ie_len = nla_len(resp_ie);
	}
	nl_get_assoc_freq(cur_if, &event.assoc.freq);

	wfs_process_event(cur_if, EVT_ASSOC, &event);
}


static void nl_send_conn_fail_event(struct wfs_iface *cur_if,
		struct nlattr *tb[])
{
	struct nlattr *addr = NULL, *reason = NULL;
	union wfs_event_data event;

	assert(cur_if && tb);

	addr = tb[NL80211_ATTR_MAC];
	reason = tb[NL80211_ATTR_CONN_FAILED_REASON];
	memset(&event, 0, sizeof(event));

	if (!addr || !reason)
		return;

	wfs_printf(LOG_DEBUG, "Connect failed event");

	memcpy(event.conn_fail.addr, nla_data(addr), ETH_ALEN);
	switch (nla_get_u32(reason)) {
	case NL80211_CONN_FAIL_MAX_CLIENTS:
		wfs_printf(LOG_DEBUG, "Max client reached");
		event.conn_fail.reason = MAX_CLIENTS;
		break;
	case NL80211_CONN_FAIL_BLOCKED_CLIENT:
		wfs_printf(LOG_DEBUG, "Blocked client "MACSTR" tried to "
				"connect", MAC2STR(event.conn_fail.addr));
		event.conn_fail.reason = BLOCKED_CLIENT;
		break;
	default:
		wfs_printf(LOG_DEBUG, "Unknown connect failed reason %u",
				nla_get_u32(reason));
		return;
	}

	wfs_process_event(cur_if, EVT_CONN_FAIL, &event);
}


static enum wfs_ch_width nl_get_ch_width(u32 width)
{
	switch (width) {
	case NL80211_CHAN_WIDTH_20_NOHT:
		return CH_WIDTH_20_NOHT;
	case NL80211_CHAN_WIDTH_20:
		return CH_WIDTH_20;
	case NL80211_CHAN_WIDTH_40:
		return CH_WIDTH_40;
	case NL80211_CHAN_WIDTH_80:
		return CH_WIDTH_80;
	case NL80211_CHAN_WIDTH_80P80:
		return CH_WIDTH_80P80;
	case NL80211_CHAN_WIDTH_160:
		return CH_WIDTH_160;
	default:
		return CH_WIDTH_UNKNOWN;
	}
}


static void nl_send_ch_switch_event(struct wfs_iface *cur_if,
		struct nlattr *tb[])
{
	struct nl80211_driver_info *info = NULL;
	struct nlattr *ifidx = NULL, *freq = NULL, *ch_type = NULL;
	struct nlattr *ch_width = NULL, *c_freq1 = NULL, *c_freq2 = NULL;
	union wfs_event_data event;

	assert(cur_if && cur_if->driver_info && tb);

	info = cur_if->driver_info;

	ifidx = tb[NL80211_ATTR_IFINDEX];
	freq = tb[NL80211_ATTR_WIPHY_FREQ];
	ch_type = tb[NL80211_ATTR_WIPHY_CHANNEL_TYPE];
	ch_width = tb[NL80211_ATTR_CHANNEL_WIDTH];
	c_freq1 = tb[NL80211_ATTR_CENTER_FREQ1];
	c_freq2 = tb[NL80211_ATTR_CENTER_FREQ2];
	memset(&event, 0, sizeof(event));

	if (!ifidx || !freq
			|| nla_get_u32(ifidx) != if_nametoindex(cur_if->ifname))
		return;

	wfs_printf(LOG_DEBUG, "Channel switch event");

	event.ch_switch.freq = nla_get_u32(freq);
	event.ch_switch.ht = 1;
	if (ch_type) {
		switch (nla_get_u32(ch_type)) {
		case NL80211_CHAN_NO_HT:
			event.ch_switch.ht = 0;
			break;
		case NL80211_CHAN_HT40PLUS:
			event.ch_switch.offset = 1;
			break;
		case NL80211_CHAN_HT40MINUS:
			event.ch_switch.offset = -1;
			break;
		default:
			break;
		}
	}
	if (ch_width)
		event.ch_switch.ch_width =
				nl_get_ch_width(nla_get_u32(ch_width));
	if (c_freq1)
		event.ch_switch.c_freq1 = nla_get_u32(c_freq1);
	if (c_freq2)
		event.ch_switch.c_freq2 = nla_get_u32(c_freq2);

	info->cur_status.freq = event.ch_switch.freq;

	wfs_process_event(cur_if, EVT_CH_SWITCH, &event);
}


static void nl_send_del_sta_event(struct wfs_iface *cur_if, struct nlattr *tb[])
{
	struct nlattr *addr = NULL;
	union wfs_event_data event;

	assert(cur_if && tb);

	addr = tb[NL80211_ATTR_MAC];
	memset(&event, 0, sizeof(event));

	if (!addr)
		return;

	memcpy(event.del_sta.peer, nla_data(addr), ETH_ALEN);

	wfs_printf(LOG_DEBUG, "Delete station (addr="MACSTR") event",
			MAC2STR(event.del_sta.peer));

	wfs_process_event(cur_if, EVT_DEL_STATION, &event);
}


static void nl_send_disconnect_event(struct wfs_iface *cur_if,
		struct nlattr *tb[])
{
	struct nl80211_driver_info *info = NULL;
	struct nlattr *reason = NULL, *by_ap = NULL;
	union wfs_event_data event;

	assert(cur_if && cur_if->driver_info && tb);

	info = cur_if->driver_info;
	reason = tb[NL80211_ATTR_REASON_CODE];
	by_ap = tb[NL80211_ATTR_DISCONNECTED_BY_AP];
	memset(&event, 0, sizeof(event));

	if (info->capa.flags & DRIVER_CAPA_FLAGS_SME) {
		wfs_printf(LOG_DEBUG, "Ignore disconnect event when using "
				"userspace SME");
		return;
	}

	nl_mark_disconnected(cur_if);
	if (reason)
		event.deauth.reason_code = nla_get_u16(reason);
	event.deauth.local = by_ap == NULL;

	wfs_process_event(cur_if, EVT_DEAUTH, &event);
}


static void nl_send_michael_mic_fail_event(struct wfs_iface *cur_if,
		struct nlattr *tb[])
{
	struct nlattr *addr = NULL, *key_seq = NULL, *key_type = NULL;
	struct nlattr *key_idx = NULL;
	union wfs_event_data event;

	assert(cur_if && tb);

	addr = tb[NL80211_ATTR_MAC];
	key_seq = tb[NL80211_ATTR_KEY_SEQ];
	key_type = tb[NL80211_ATTR_KEY_TYPE];
	key_idx = tb[NL80211_ATTR_KEY_IDX];
	memset(&event, 0, sizeof(event));

	if (addr) {
		memcpy(event.michael_mic_fail.src, nla_data(addr), ETH_ALEN);
		wfs_printf(LOG_DEBUG, "Source MAC address "MACSTR,
				MAC2STR(event.michael_mic_fail.src));
	}
	if (key_seq)
		wfs_hexdump(LOG_DEBUG, "TSC", nla_data(key_seq),
				nla_len(key_seq));
	if (key_type) {
		enum nl80211_key_type type = nla_get_u32(key_type);
		wfs_printf(LOG_DEBUG, "Key type %u", type);
		if (type == NL80211_KEYTYPE_PAIRWISE)
			event.michael_mic_fail.unicast = 1;
	} else {
		event.michael_mic_fail.unicast = 1;
	}
	if (key_idx)
		wfs_printf(LOG_DEBUG, "Key ID %u", nla_get_u8(key_idx));

	wfs_process_event(cur_if, EVT_MICHAEL_MIC_FAIL, &event);
}


static int link_signal_handler(struct nl_msg *msg, void *arg)
{
	struct nlattr *tb[NL80211_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nlattr *sinfo[NL80211_STA_INFO_MAX + 1];
	static struct nla_policy policy[NL80211_STA_INFO_MAX + 1] = {
		[NL80211_STA_INFO_SIGNAL] = { .type = NLA_U8 },
		[NL80211_STA_INFO_SIGNAL_AVG] = { .type = NLA_U8 },
	};
	struct nlattr *rinfo[NL80211_RATE_INFO_MAX + 1];
	static struct nla_policy rate_policy[NL80211_RATE_INFO_MAX + 1] = {
		[NL80211_RATE_INFO_BITRATE] = { .type = NLA_U16 },
		[NL80211_RATE_INFO_MCS] = { .type = NLA_U8 },
		[NL80211_RATE_INFO_40_MHZ_WIDTH] = { .type = NLA_FLAG },
		[NL80211_RATE_INFO_SHORT_GI] = { .type = NLA_FLAG },
	};
	struct nlattr *sur_info[NL80211_SURVEY_INFO_MAX + 1];
	static struct nla_policy survey_policy[NL80211_SURVEY_INFO_MAX + 1] = {
		[NL80211_SURVEY_INFO_FREQUENCY] = { .type = NLA_U32 },
		[NL80211_SURVEY_INFO_NOISE] = { .type = NLA_U8 },
	};
	struct nlattr *ch_width = NULL, *c_freq1 = NULL, *c_freq2 = NULL;
	struct sig_data *sig = arg;

	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
		  genlmsg_attrlen(gnlh, 0), NULL);
	if (!tb[NL80211_ATTR_STA_INFO]
		|| nla_parse_nested(sinfo, NL80211_STA_INFO_MAX,
				tb[NL80211_ATTR_STA_INFO], policy))
		return NL_SKIP;
	if (!sinfo[NL80211_STA_INFO_SIGNAL])
		return NL_SKIP;

	sig->sig = nla_get_u8(sinfo[NL80211_STA_INFO_SIGNAL]);

	if (sinfo[NL80211_STA_INFO_SIGNAL_AVG])
		sig->avg_sig = nla_get_u8(sinfo[NL80211_STA_INFO_SIGNAL_AVG]);

	if (sinfo[NL80211_STA_INFO_TX_BITRATE]) {
		if (nla_parse_nested(rinfo, NL80211_RATE_INFO_MAX,
			sinfo[NL80211_STA_INFO_TX_BITRATE], rate_policy))
			sig->tx_rate = 0;
		else if (rinfo[NL80211_RATE_INFO_BITRATE])
			sig->tx_rate =
			nla_get_u16(rinfo[NL80211_RATE_INFO_BITRATE]) * 100;
	}

	if (!tb[NL80211_ATTR_SURVEY_INFO]) {
		wfs_printf(LOG_DEBUG, "Survey data missing");
		return NL_SKIP;
	}

	if (nla_parse_nested(sur_info, NL80211_SURVEY_INFO_MAX,
			tb[NL80211_ATTR_SURVEY_INFO], survey_policy))
		return NL_SKIP;

	if (!sur_info[NL80211_SURVEY_INFO_FREQUENCY])
		return NL_SKIP;

	if (nla_get_u32(sur_info[NL80211_SURVEY_INFO_FREQUENCY]) != sig->freq)
		return NL_SKIP;

	if (!sur_info[NL80211_SURVEY_INFO_NOISE])
		return NL_SKIP;

	sig->noise = nla_get_u8(sinfo[NL80211_SURVEY_INFO_NOISE]);

	ch_width = tb[NL80211_ATTR_CHANNEL_WIDTH];
	if (ch_width) {
		sig->ch_width = nl_get_ch_width(nla_get_u32(ch_width));
		c_freq1 = tb[NL80211_ATTR_CENTER_FREQ1];
		if (c_freq1)
			sig->c_freq1 = nla_get_u32(c_freq1);
		c_freq2 = tb[NL80211_ATTR_CENTER_FREQ2];
		if (c_freq2)
			sig->c_freq2 = nla_get_u32(c_freq2);
	}

	return NL_SKIP;
}


static enum wfs_internal_err
nl_get_link_signal(struct wfs_iface *cur_if, struct sig_data *sig)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if && sig);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);

	add_genl_header(msg, info->family_id, NLM_F_DUMP, NL80211_CMD_GET_STATION);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);
	NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, info->cur_status.bssid);

	sig->freq = info->cur_status.freq;
	err = send_and_recv(cur_if, msg, link_signal_handler, sig);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to get link signal: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Get link signal successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static void nl_send_new_sta_event(struct wfs_iface *cur_if, struct nlattr *tb[])
{
	struct nlattr *addr = NULL;
	union wfs_event_data event;

	assert(cur_if && tb);

	addr = tb[NL80211_ATTR_MAC];
	memset(&event, 0, sizeof(event));

	if (!addr)
		return;

	memcpy(event.new_sta.peer, nla_data(addr), ETH_ALEN);

	wfs_printf(LOG_DEBUG, "New station (addr="MACSTR") event",
			MAC2STR(event.new_sta.peer));

	wfs_process_event(cur_if, EVT_NEW_STATION, &event);
}


static void nl_send_cqm_event(struct wfs_iface *cur_if, struct nlattr *tb[])
{
	static struct nla_policy cqm_policy[NL80211_ATTR_CQM_MAX + 1] = {
		[NL80211_ATTR_CQM_RSSI_THOLD] = { .type = NLA_U32 },
		[NL80211_ATTR_CQM_RSSI_HYST] = { .type = NLA_U8 },
		[NL80211_ATTR_CQM_RSSI_THRESHOLD_EVENT] = { .type = NLA_U32 },
		[NL80211_ATTR_CQM_PKT_LOSS_EVENT] = { .type = NLA_U32 },
	};
	struct nlattr *cqm[NL80211_ATTR_CQM_MAX + 1];
	enum nl80211_cqm_rssi_threshold_event rssi_ev;
	struct sig_data sig;
	union wfs_event_data event;
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;

	assert(cur_if && tb);

	memset(&event, 0, sizeof(event));
	memset(&sig, 0, sizeof(sig));

	if (!tb[NL80211_ATTR_CQM]
			|| nla_parse_nested(cqm, NL80211_ATTR_CQM_MAX,
				tb[NL80211_ATTR_CQM], cqm_policy)) {
		wfs_printf(LOG_DEBUG, "Ignore invalid CQM event");
		return;
	}
	if (cqm[NL80211_ATTR_CQM_PKT_LOSS_EVENT]) {
		if (!tb[NL80211_ATTR_MAC])
			return;
		memcpy(event.peer_low_ack.addr, nla_data(tb[NL80211_ATTR_MAC]),
				ETH_ALEN);
		wfs_process_event(cur_if, EVT_PEER_LOW_ACK, &event);
		return;
	}
	if (!cqm[NL80211_ATTR_CQM_RSSI_THRESHOLD_EVENT])
		return;
	rssi_ev = nla_get_u32(cqm[NL80211_ATTR_CQM_RSSI_THRESHOLD_EVENT]);
	if (rssi_ev == NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH) {
		wfs_printf(LOG_DEBUG, "CQM event: RSSI high");
		event.sig_change.above_threshold = 1;
	} else if (rssi_ev == NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW) {
		wfs_printf(LOG_DEBUG, "CQM event: RSSI low");
		event.sig_change.above_threshold = 0;
	} else {
		return;
	}

	ret = nl_get_link_signal(cur_if, &sig);
	if (ret == WFS_INTERNAL_ERR_SUCCESS) {
		event.sig_change.sig = sig.sig;
		event.sig_change.tx_rate = sig.tx_rate;
		event.sig_change.noise = sig.noise;
		wfs_printf(LOG_DEBUG, "Signal: %u dBm; TX rate: %u; "
				"Noise: %u dBm", sig.sig, sig.tx_rate,
				sig.noise);
	}

	wfs_process_event(cur_if, EVT_SIGNAL_CHANGE, &event);
}


static void nl_send_probe_client_event(struct wfs_iface *cur_if,
		struct nlattr *tb[])
{
	struct nlattr *addr = NULL, *ack = NULL;
	union wfs_event_data event;

	assert(cur_if && tb);

	addr = tb[NL80211_ATTR_MAC];
	ack = tb[NL80211_ATTR_ACK];
	memset(&event, 0, sizeof(event));

	if (!addr || !ack)
		return;
	memcpy(event.probe_client.addr, nla_data(addr), ETH_ALEN);

	wfs_process_event(cur_if, EVT_PROBE_CLIENT_OK, &event);
}


static enum wfs_event_type
nl_get_dfs_event(enum nl80211_radar_event event)
{
	switch (event) {
	case NL80211_RADAR_DETECTED:
		return EVT_DFS_RADAR_DETECTED;
	case NL80211_RADAR_CAC_FINISHED:
		return EVT_DFS_CAC_FINISHED;
	case NL80211_RADAR_CAC_ABORTED:
		return EVT_DFS_CAC_ABORTED;
	case NL80211_RADAR_NOP_FINISHED:
		return EVT_DFS_NOP_FINISHED;
	default:
		return EVT_UNKNOWN;
	}
}


static void nl_send_dfs_event(struct wfs_iface *cur_if, struct nlattr *tb[])
{
	struct nlattr *freq = NULL, *radar_event = NULL, *ch_type = NULL;
	struct nlattr *ch_width = NULL, *c_freq1 = NULL, *c_freq2 = NULL;
	union wfs_event_data event;

	assert(cur_if && tb);

	freq = tb[NL80211_ATTR_WIPHY_FREQ];
	radar_event = tb[NL80211_ATTR_RADAR_EVENT];
	ch_type = tb[NL80211_ATTR_WIPHY_CHANNEL_TYPE];
	ch_width = tb[NL80211_ATTR_CHANNEL_WIDTH];
	c_freq1 = tb[NL80211_ATTR_CENTER_FREQ1];
	c_freq2 = tb[NL80211_ATTR_CENTER_FREQ2];
	memset(&event, 0, sizeof(event));

	if (!freq || !radar_event)
		return;

	event.dfs.freq = nla_get_u32(freq);

	event.ch_switch.ht = 1;
	if (ch_type) {
		switch (nla_get_u32(ch_type)) {
		case NL80211_CHAN_NO_HT:
			event.dfs.ht = 0;
			break;
		case NL80211_CHAN_HT40PLUS:
			event.dfs.offset = 1;
			break;
		case NL80211_CHAN_HT40MINUS:
			event.dfs.offset = -1;
			break;
		default:
			break;
		}
	}
	if (ch_width)
		event.dfs.ch_width = nl_get_ch_width(nla_get_u32(ch_width));
	if (c_freq1)
		event.dfs.c_freq1 = nla_get_u32(c_freq1);
	if (c_freq2)
		event.dfs.c_freq2 = nla_get_u32(c_freq2);

	wfs_printf(LOG_DEBUG, "DFS event on freq %u MHz%soffset: %d, "
			"channel width: %s, c_freq1: %u MHz, c_freq2: %u MHz",
			event.dfs.freq, event.dfs.ht ? " (HT), " : ", ",
			event.dfs.offset, get_ch_width_str(event.dfs.ch_width),
			event.dfs.c_freq1, event.dfs.c_freq2);

	wfs_process_event(cur_if, nl_get_dfs_event(nla_get_u32(radar_event)),
			&event);
}


static void nl_send_reg_change_event(struct wfs_iface *cur_if,
		struct nlattr *tb[])
{
	struct nlattr *initiator = NULL;
	union wfs_event_data event;

	assert(cur_if && tb);

	initiator = tb[NL80211_ATTR_REG_INITIATOR];
	memset(&event, 0, sizeof(event));

	if (!initiator)
		return;

	wfs_printf(LOG_DEBUG, "Regulatory domain change event");

	switch (nla_get_u8(initiator)) {
	case NL80211_REGDOM_SET_BY_CORE:
		event.reg_change.initiator = REGDOM_SET_BY_CORE;
		break;
	case NL80211_REGDOM_SET_BY_USER:
		event.reg_change.initiator = REGDOM_SET_BY_USER;
		break;
	case NL80211_REGDOM_SET_BY_DRIVER:
		event.reg_change.initiator = REGDOM_SET_BY_DRIVER;
		break;
	case NL80211_REGDOM_SET_BY_COUNTRY_IE:
		event.reg_change.initiator = REGDOM_SET_BY_COUNTRY_IE;
		break;
	default:
		wfs_printf(LOG_DEBUG, "Unknown regulatory change initiator %u",
				nla_get_u8(initiator));
		break;
	}

	wfs_process_event(cur_if, EVT_REG_CHANGE, &event);
}


static void nl_send_roc_event(struct wfs_iface *cur_if, u8 cancel,
		struct nlattr *tb[])
{
	struct nl80211_driver_info *info = NULL;
	struct nlattr *freq = NULL, *type = NULL, *dur = NULL, *cookie = NULL;
	u32 chan_type = 0;
	u64 roc_cookie = 0;
	union wfs_event_data event;

	assert(cur_if && cur_if->driver_info && tb);

	info = cur_if->driver_info;
	freq = tb[NL80211_ATTR_WIPHY_FREQ];
	type = tb[NL80211_ATTR_WIPHY_CHANNEL_TYPE];
	dur = tb[NL80211_ATTR_DURATION];
	cookie = tb[NL80211_ATTR_COOKIE];
	memset(&event, 0, sizeof(event));

	if (freq)
		event.roc.freq = nla_get_u32(freq);
	if (type)
		chan_type = nla_get_u32(type);
	if (dur)
		event.roc.dur = nla_get_u32(dur);
	if (cookie)
		roc_cookie = nla_get_u64(cookie);

	wfs_printf(LOG_DEBUG, "Remain-on-channel event (cancel=%u freq=%u "
		"channel_type=%u duration=%u cookie=0x%llx (%s))",
		cancel, event.roc.freq, chan_type, event.roc.dur,
		(long long unsigned int) roc_cookie,
		roc_cookie == info->cur_status.roc.cookie ?
			"match" : "unknown");

	if (roc_cookie != info->cur_status.roc.cookie)
		return;

	if (cancel)
		info->cur_status.roc.is_pending = 0;

	wfs_process_event(cur_if, cancel ? EVT_CANCEL_ROC: EVT_ROC, &event);
}


static void nl_send_scan_event(struct wfs_iface *cur_if, int aborted,
		struct nlattr *tb[])
{
	struct nlattr *scan_ssids = NULL, *scan_freqs = NULL;
	union wfs_event_data event;
#define MAX_REPORT_FREQS 50
	u32 freqs[MAX_REPORT_FREQS];
	u32 freq_num = 0;

	assert(cur_if && tb);

	scan_ssids = tb[NL80211_ATTR_SCAN_SSIDS];
	scan_freqs = tb[NL80211_ATTR_SCAN_FREQUENCIES];
	memset(&event, 0, sizeof(event));
	memset(freqs, 0, sizeof(freqs));

	event.scan.aborted = aborted;

	if (scan_ssids) {
		struct nlattr *nl = NULL;
		int rem = 0;
		nla_for_each_nested(nl, scan_ssids, rem) {
			struct scanned_device *s =
				&event.scan.devices[event.scan.device_num];
			s->ssid = nla_data(nl);
			s->ssid_len = nla_len(nl);
			wfs_printf(LOG_DEBUG, "Scan probed for SSID %s",
					get_ssid_str(s->ssid, s->ssid_len));
			++event.scan.device_num;
			if (event.scan.device_num == MAX_SCAN_DEVICES)
				break;
		}
	}

	if (scan_freqs) {
		struct nlattr *nl = NULL;
		int rem = 0;
		nla_for_each_nested(nl, scan_freqs, rem) {
			freqs[freq_num] = nla_get_u32(nl);
			++freq_num;
			if (freq_num == MAX_REPORT_FREQS)
				break;
		}
		event.scan.freqs = freqs;
		event.scan.freq_num = freq_num;
		wfs_printf(LOG_DEBUG, "Scanned frequencies: %s",
				get_u32_array_str(freqs, freq_num, " "));
	}

	wfs_process_event(cur_if, EVT_SCAN_RESULTS, &event);
}


static enum wfs_tdls_oper nl_get_tdls_oper(enum nl80211_tdls_operation oper)
{
	switch(oper) {
	case NL80211_TDLS_DISCOVERY_REQ:
		return TDLS_DISC_REQ;
	case NL80211_TDLS_SETUP:
		return TDLS_SETUP;
	case NL80211_TDLS_TEARDOWN:
		return TDLS_TEARDOWN;
	case NL80211_TDLS_ENABLE_LINK:
		return TDLS_ENABLE_LINK;
	case NL80211_TDLS_DISABLE_LINK:
		return TDLS_DISABLE_LINK;
	default:
		return TDLS_UNKNOWN;
	}
}


static void nl_send_tdls_oper_event(struct wfs_iface *cur_if,
		struct nlattr *tb[])
{
	struct nlattr *addr = NULL, *oper = NULL, *reason = NULL;
	union wfs_event_data event;

	assert(cur_if && tb);

	addr = tb[NL80211_ATTR_MAC];
	oper = tb[NL80211_ATTR_TDLS_OPERATION];
	reason = tb[NL80211_ATTR_REASON_CODE];
	memset(&event, 0, sizeof(event));

	if (!addr || !oper)
		return;

	wfs_printf(LOG_DEBUG, "TDLS operation event");

	memcpy(event.tdls.peer, nla_data(addr), ETH_ALEN);
	event.tdls.oper = nl_get_tdls_oper(nla_get_u8(oper));
	wfs_printf(LOG_DEBUG, "TDLS request %s for peer "MACSTR,
			get_tdls_oper_str(event.tdls.oper),
			MAC2STR(event.tdls.peer));
	if (event.tdls.oper == TDLS_UNKNOWN)
		return;
	if (reason)
		event.tdls.reason_code = nla_get_u16(reason);

	wfs_process_event(cur_if, EVT_TDLS, &event);
}


static void nl_scan_timer(void *ctx)
{
	struct wfs_iface *cur_if = ctx;

	assert(ctx);

	ENTER();

	wfs_printf(LOG_DEBUG, "Scan timeout - try to get results");

	wfs_process_event(cur_if, EVT_SCAN_RESULTS, NULL);

	LEAVE();
}


static int nl_process_event(struct nl_msg *msg, void *arg)
{
	struct wfs_iface *cur_if = arg;
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nlattr *tb[NL80211_ATTR_MAX + 1];
	struct nl80211_driver_info *info = NULL;

	assert(msg && arg);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto done;

	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
			genlmsg_attrlen(gnlh, 0), NULL);

	if (tb[NL80211_ATTR_IFINDEX]) {
		u32 if_idx = nla_get_u32(tb[NL80211_ATTR_IFINDEX]);
		if (if_idx != if_nametoindex(cur_if->ifname))
			goto done;
	}

	wfs_printf(LOG_DEBUG, "Driver event %d (%s) received for %s",
			gnlh->cmd, nl80211_cmd_to_str(gnlh->cmd),
			cur_if->ifname);

	switch (gnlh->cmd) {
	case NL80211_CMD_AUTHENTICATE:
	case NL80211_CMD_ASSOCIATE:
	case NL80211_CMD_DEAUTHENTICATE:
	case NL80211_CMD_DISASSOCIATE:
	case NL80211_CMD_FRAME_TX_STATUS:
	case NL80211_CMD_UNPROT_DEAUTHENTICATE:
	case NL80211_CMD_UNPROT_DISASSOCIATE:
		nl_send_mlme_event(cur_if, gnlh->cmd, tb);
		break;
	case NL80211_CMD_CANCEL_REMAIN_ON_CHANNEL:
		nl_send_roc_event(cur_if, 1, tb);
		break;
	case NL80211_CMD_CH_SWITCH_NOTIFY:
		nl_send_ch_switch_event(cur_if, tb);
		break;
	case NL80211_CMD_CONNECT:
	case NL80211_CMD_ROAM:
		nl_send_connect_event(cur_if, gnlh->cmd, tb);
		break;
	case NL80211_CMD_CONN_FAILED:
		nl_send_conn_fail_event(cur_if, tb);
		break;
	case NL80211_CMD_DEL_STATION:
		nl_send_del_sta_event(cur_if, tb);
		break;
	case NL80211_CMD_DISCONNECT:
		nl_send_disconnect_event(cur_if, tb);
		break;
	case NL80211_CMD_MICHAEL_MIC_FAILURE:
		nl_send_michael_mic_fail_event(cur_if, tb);
		break;
	case NL80211_CMD_NEW_SCAN_RESULTS:
		info->cur_status.scan.scan_state = SCAN_COMPLETED;
		wfs_loop_unregister_timer(nl_scan_timer, cur_if);
		nl_send_scan_event(cur_if, 0, tb);
		break;
	case NL80211_CMD_NEW_STATION:
		nl_send_new_sta_event(cur_if, tb);
		break;
	case NL80211_CMD_NOTIFY_CQM:
		nl_send_cqm_event(cur_if, tb);
		break;
	case NL80211_CMD_PROBE_CLIENT:
		nl_send_probe_client_event(cur_if, tb);
		break;
	case NL80211_CMD_RADAR_DETECT:
		nl_send_dfs_event(cur_if, tb);
		break;
	case NL80211_CMD_REG_BEACON_HINT:
		wfs_printf(LOG_DEBUG, "Regulatory beacon hint event");
		wfs_process_event(cur_if, EVT_REG_CHANGE, NULL);
		break;
	case NL80211_CMD_REG_CHANGE:
		nl_send_reg_change_event(cur_if, tb);
		break;
	case NL80211_CMD_REMAIN_ON_CHANNEL:
		nl_send_roc_event(cur_if, 0, tb);
		break;
	case NL80211_CMD_SCAN_ABORTED:
		info->cur_status.scan.scan_state = SCAN_ABORTED;
		wfs_loop_unregister_timer(nl_scan_timer, cur_if);
		nl_send_scan_event(cur_if, 1, tb);
		break;
	case NL80211_CMD_SCHED_SCAN_RESULTS:
		info->cur_status.scan.scan_state = SCHED_SCAN_RESULTS;
		nl_send_scan_event(cur_if, 0, tb);
		break;
	case NL80211_CMD_SCHED_SCAN_STOPPED:
		info->cur_status.scan.scan_state = SCHED_SCAN_STOPPED;
		wfs_process_event(cur_if, EVT_SCHED_SCAN_STOPPED, NULL);
		break;
	case NL80211_CMD_START_SCHED_SCAN:
		info->cur_status.scan.scan_state = SCHED_SCAN_STARTED;
		wfs_process_event(cur_if, EVT_START_SCHED_SCAN, NULL);
		break;
	case NL80211_CMD_STOP_AP:
		wfs_process_event(cur_if, EVT_STOP_AP, NULL);
		break;
	case NL80211_CMD_TDLS_OPER:
		nl_send_tdls_oper_event(cur_if, tb);
		break;
	case NL80211_CMD_TRIGGER_SCAN:
		info->cur_status.scan.scan_state = SCAN_STARTED;
		wfs_process_event(cur_if, EVT_TRIGGER_SCAN, NULL);
		break;
	default:
		wfs_printf(LOG_DEBUG, "Unsupported driver event %d (%s)",
				gnlh->cmd, nl80211_cmd_to_str(gnlh->cmd));
		break;
	}

done:
	LEAVE();
	return NL_SKIP;
}


static void event_receive(int sock, void *ctx)
{
	struct nl80211_driver_info *info = ctx;

	assert(ctx);

	ENTER();

	wfs_printf(LOG_DEBUG, "Event message available");

	nl_recvmsgs(info->evt_handle, info->nl_cb);

	LEAVE();
}


static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *nlerr,
		void *arg)
{
	int *ret = arg;

	assert(nlerr && arg);

	*ret = nlerr->error;

	return NL_SKIP;
}


static int finish_handler(struct nl_msg *msg, void *arg)
{
	int *ret = arg;

	assert(msg && arg);

	*ret = 0;

	return NL_SKIP;
}


static int ack_handler(struct nl_msg *msg, void *arg)
{
	int *ret = arg;

	assert(msg && arg);

	*ret = 0;

	return NL_SKIP;
}


static int send_and_recv(struct wfs_iface *cur_if, struct nl_msg *msg,
		int(*valid_handler)(struct nl_msg *, void *), void *valid_arg)
{
	int err = -ENOMEM;
	struct nl_cb *cb = NULL;
	struct nl80211_driver_info *info = NULL;

	assert(cur_if && msg);

	ENTER();

	info = cur_if->driver_info;

	cb = nl_cb_clone(info->nl_cb);
	if (!cb)
		goto out;

	err = nl_send_auto_complete(info->cmd_handle, msg);
	if (err < 0)
		goto out;

	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);

	if (valid_handler)
		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, valid_handler,
				valid_arg);

	while (err > 0)
		nl_recvmsgs(info->cmd_handle, cb);

out:
	nl_cb_put(cb);
	nlmsg_free(msg);
	LEAVE();
	return err;
}


static const char *iftype_to_str(enum nl80211_iftype nl_iftype)
{
	switch (nl_iftype) {
	case NL80211_IFTYPE_ADHOC:
		return "ADHOC";
	case NL80211_IFTYPE_STATION:
		return "STATION";
	case NL80211_IFTYPE_AP:
		return "AP";
	case NL80211_IFTYPE_AP_VLAN:
		return "AP_VLAN";
	case NL80211_IFTYPE_WDS:
		return "WFS";
	case NL80211_IFTYPE_MONITOR:
		return "MONITOR";
	case NL80211_IFTYPE_MESH_POINT:
		return "MESH_POINT";
	case NL80211_IFTYPE_P2P_CLIENT:
		return "P2P_CLIENT";
	case NL80211_IFTYPE_P2P_GO:
		return "P2P_GO";
	case NL80211_IFTYPE_P2P_DEVICE:
		return "P2P_DEVICE";
	default:
		wfs_printf(LOG_DEBUG, "Unknown iftype: %d", nl_iftype);
		return "UNKNOWN";
	}
}


static enum wfs_internal_err
nl_send_mlme_with_reason_code(struct wfs_iface *cur_if, const u8 *addr, u8 cmd,
		u16 reason_code, u8 local_state_change)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if && addr);

	ENTER();

	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);

	add_genl_header(msg, info->family_id, 0, cmd);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);
	NLA_PUT_U16(msg, NL80211_ATTR_REASON_CODE, reason_code);

	if (!is_zero_mac_addr(addr))
		NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, addr);
	if (local_state_change)
		NLA_PUT_FLAG(msg, NL80211_ATTR_LOCAL_STATE_CHANGE);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to send MLME command: reason=%u "
				"err=%d (%s)", reason_code ,err,
				strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static struct nl_msg *
nl_build_scan_msg(struct wfs_iface *cur_if, u8 cmd,
		struct wfs_driver_scan_params *params)
{
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	u32 if_idx = 0;
	int i = 0;

	assert(cur_if && params);

	ENTER();

	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		wfs_printf(LOG_ERR, "Failed to allocate netlink message.");
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);

	add_genl_header(msg, info->family_id, 0, cmd);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	if (params->device_num) {
		struct nlattr *ssids =
			nla_nest_start(msg, NL80211_ATTR_SCAN_SSIDS);
		if (!ssids)
			goto nla_put_failure;
		for (i = 0; i < params->device_num; ++i) {
			wfs_hexdump_ascii(LOG_DEBUG, "Scan device",
					params->devices[i].ssid,
					params->devices[i].ssid_len);
			NLA_PUT(msg, i + 1, params->devices[i].ssid_len,
					params->devices[i].ssid);
		}
		nla_nest_end(msg, ssids);
	}

	if (params->ie && params->ie_len) {
		wfs_hexdump(LOG_DEBUG, "Scan IEs", params->ie, params->ie_len);
		NLA_PUT(msg, NL80211_ATTR_IE, params->ie_len, params->ie);
	}

	if (params->freqs) {
		struct nlattr *freqs =
			nla_nest_start(msg, NL80211_ATTR_SCAN_FREQUENCIES);
		if (!freqs)
			goto nla_put_failure;
		for (i = 0; i < params->freq_num; ++i) {
			wfs_printf(LOG_DEBUG, "Scan frequency %u MHz",
					params->freqs[i]);
			NLA_PUT_U32(msg, i + 1, params->freqs[i]);
		}
		nla_nest_end(msg, freqs);
	}

	FREE(info->cur_status.scan.match_devs);
	info->cur_status.scan.match_devs = mem_dup(params->match_devs,
			sizeof(*params->match_devs) * params->match_devs_num);
	info->cur_status.scan.match_devs_num = params->match_devs_num;

	if (params->only_new_results) {
		wfs_printf(LOG_DEBUG, "Add NL80211_SCAN_FLAG_FLUSH");
		NLA_PUT_U32(msg, NL80211_ATTR_SCAN_FLAGS,
			    NL80211_SCAN_FLAG_FLUSH);
	}

	goto out;

nla_put_failure:
	nlmsg_free(msg);
	msg = NULL;
out:
	LEAVE();
	return msg;
}


static int cookie_handler(struct nl_msg *msg, void *arg)
{
	struct nlattr *tb[NL80211_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	u64 *cookie = arg;
	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
		  genlmsg_attrlen(gnlh, 0), NULL);
	if (tb[NL80211_ATTR_COOKIE])
		*cookie = nla_get_u64(tb[NL80211_ATTR_COOKIE]);
	return NL_SKIP;
}


static enum nl80211_iftype get_mapping_iftype(enum wfs_driver_if_type type)
{
	switch (type) {
	case WFS_DRV_IF_TYPE_ADHOC:
		return NL80211_IFTYPE_ADHOC;
	case WFS_DRV_IF_TYPE_STATION:
		return NL80211_IFTYPE_STATION;
	case WFS_DRV_IF_TYPE_AP_VLAN:
		return NL80211_IFTYPE_AP_VLAN;
	case WFS_DRV_IF_TYPE_AP_BSS:
		return NL80211_IFTYPE_AP;
	case WFS_DRV_IF_TYPE_P2P_CLIENT:
	case WFS_DRV_IF_TYPE_P2P_GROUP:
		return NL80211_IFTYPE_P2P_CLIENT;
	case WFS_DRV_IF_TYPE_P2P_GO:
		return NL80211_IFTYPE_P2P_GO;
	case WFS_DRV_IF_TYPE_P2P_DEVICE:
		return NL80211_IFTYPE_P2P_DEVICE;
	default:
		wfs_printf(LOG_ERR, "Unsupported interface type: %d", type);
		return -1;
	}
}


static enum wfs_internal_err
nl_add_key(struct nl_msg *msg, int def_key,
		struct wfs_driver_key_params *params)
{
	struct nlattr *key_attr = nla_nest_start(msg, NL80211_ATTR_KEY);
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;

	if (!key_attr) {
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	if (def_key)
		NLA_PUT_FLAG(msg, (params->cipher == CIPHER_IGTK)
				? NL80211_KEY_DEFAULT_MGMT
				: NL80211_KEY_DEFAULT);

	NLA_PUT_U8(msg, NL80211_KEY_IDX, params->key_idx);

	if (params->cipher) {
		int cipher = get_cipher_suite(params->cipher);
		if (cipher == 0) {
			ret = WFS_INTERNAL_ERR_INVAL;
			goto nla_put_failure;
		}
		NLA_PUT_U32(msg, NL80211_KEY_CIPHER, cipher);
	}

	if (params->seq && params->seq_len)
		NLA_PUT(msg, NL80211_KEY_SEQ, params->seq_len, params->seq);

	NLA_PUT(msg, NL80211_KEY_DATA, params->key_len, params->key);

	nla_nest_end(msg, key_attr);

nla_put_failure:
	return ret;
}


static enum wfs_internal_err
nl_disconnect(struct wfs_iface *cur_if, u16 reason_code)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;

	assert(cur_if);

	ENTER();

	nl_mark_disconnected(cur_if);

	wfs_printf(LOG_DEBUG, "%s(reason_code=%d)", __func__, reason_code);
	ret = nl_send_mlme_with_reason_code(cur_if, zero_addr,
			NL80211_CMD_DISCONNECT, reason_code, 0);

	LEAVE();
	return ret;
}


static int scan_matched(struct nl80211_driver_info *info, const u8 *ie,
		u32 ie_len)
{
	const u8 *ssid = NULL;
	u32 ssid_len = 0, i = 0;

	if (!info->cur_status.scan.match_devs)
		return TRUE;
	if (!ie)
		return FALSE;
	ssid = get_ie_ssid(ie, ie_len, &ssid_len);
	if (!ssid)
		return FALSE;
	for (i = 0; i < info->cur_status.scan.match_devs_num; ++i) {
		if (ssid_len == info->cur_status.scan.match_devs[i].ssid_len
			&& memcmp(ssid,
				info->cur_status.scan.match_devs[i].ssid,
				ssid_len) == 0)
			return TRUE;
	}

	return FALSE;
}


static int bss_info_handler(struct nl_msg *msg, void *arg)
{
	struct nl80211_bss_info *bss_info = arg;
	struct nlattr *tb[NL80211_ATTR_MAX + 1];
	struct nlattr *bss[NL80211_BSS_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nla_policy bss_policy[NL80211_BSS_MAX + 1] = {
		[NL80211_BSS_BSSID] = { .type = NLA_UNSPEC },
		[NL80211_BSS_FREQUENCY] = { .type = NLA_U32 },
		[NL80211_BSS_TSF] = { .type = NLA_U64 },
		[NL80211_BSS_BEACON_INTERVAL] = { .type = NLA_U16 },
		[NL80211_BSS_CAPABILITY] = { .type = NLA_U16 },
		[NL80211_BSS_INFORMATION_ELEMENTS] = { .type = NLA_UNSPEC },
		[NL80211_BSS_SIGNAL_MBM] = { .type = NLA_U32 },
		[NL80211_BSS_SIGNAL_UNSPEC] = { .type = NLA_U8 },
		[NL80211_BSS_STATUS] = { .type = NLA_U32 },
		[NL80211_BSS_SEEN_MS_AGO] = { .type = NLA_U32 },
		[NL80211_BSS_BEACON_IES] = { .type = NLA_UNSPEC },
	};
	struct wfs_driver_scan_results *scan_results = bss_info->scan_results;
	const u8 *ie = NULL, *beacon_ie = NULL;
	u32 ie_len = 0, beacon_ie_len = 0;
	struct wfs_driver_scan_result *res = NULL, **tmp = NULL;

	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
		  genlmsg_attrlen(gnlh, 0), NULL);
	if (!tb[NL80211_ATTR_BSS])
		goto done;
	if (nla_parse_nested(bss, NL80211_BSS_MAX, tb[NL80211_ATTR_BSS],
				bss_policy))
		goto done;
	if (bss[NL80211_BSS_STATUS]) {
		enum nl80211_bss_status status =
			nla_get_u32(bss[NL80211_BSS_STATUS]);
		if (status == NL80211_BSS_STATUS_ASSOCIATED &&
				bss[NL80211_BSS_FREQUENCY]) {
			bss_info->assoc_freq =
				nla_get_u32(bss[NL80211_BSS_FREQUENCY]);
			wfs_printf(LOG_DEBUG, "Associated on %u MHz",
					bss_info->assoc_freq);
		}
		if (status == NL80211_BSS_STATUS_ASSOCIATED &&
				bss[NL80211_BSS_BSSID]) {
			memcpy(bss_info->assoc_bssid,
					nla_data(bss[NL80211_BSS_BSSID]),
					ETH_ALEN);
			wfs_printf(LOG_DEBUG, "Associated with " MACSTR,
					MAC2STR(bss_info->assoc_bssid));
		}
	}
	if (!scan_results)
		goto done;
	if (bss[NL80211_BSS_INFORMATION_ELEMENTS]) {
		ie = nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]);
		ie_len = nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS]);
	}
	if (bss[NL80211_BSS_BEACON_IES]) {
		beacon_ie = nla_data(bss[NL80211_BSS_BEACON_IES]);
		beacon_ie_len = nla_len(bss[NL80211_BSS_BEACON_IES]);
	}
	if (!scan_matched(bss_info->info, ie ? ie : beacon_ie,
				ie ? ie_len : beacon_ie_len))
		goto done;
	res = zalloc(sizeof(*res));
	if (!res)
		goto done;
	if (bss[NL80211_BSS_BSSID])
		memcpy(res->bssid, nla_data(bss[NL80211_BSS_BSSID]), ETH_ALEN);
	if (bss[NL80211_BSS_FREQUENCY])
		res->freq = nla_get_u32(bss[NL80211_BSS_FREQUENCY]);
	if (bss[NL80211_BSS_BEACON_INTERVAL])
		res->beacon_interval =
			nla_get_u16(bss[NL80211_BSS_BEACON_INTERVAL]);
	if (bss[NL80211_BSS_CAPABILITY])
		res->caps = nla_get_u16(bss[NL80211_BSS_CAPABILITY]);
	res->flags |= WFS_SCAN_NOISE_INVALID;
	if (bss[NL80211_BSS_SIGNAL_MBM]) {
		res->signal_level = nla_get_u32(bss[NL80211_BSS_SIGNAL_MBM]);
		/** mBm to dBm */
		res->signal_level /= 100;
		res->flags |= WFS_SCAN_LEVEL_DBM | WFS_SCAN_QUAL_INVALID;
	} else if (bss[NL80211_BSS_SIGNAL_UNSPEC]) {
		res->signal_level = nla_get_u8(bss[NL80211_BSS_SIGNAL_UNSPEC]);
		res->flags |= WFS_SCAN_QUAL_INVALID;
	} else {
		res->flags |= WFS_SCAN_LEVEL_INVALID | WFS_SCAN_QUAL_INVALID;
	}
	if (bss[NL80211_BSS_TSF])
		res->tsf = nla_get_u64(bss[NL80211_BSS_TSF]);
	if (bss[NL80211_BSS_SEEN_MS_AGO])
		res->ago = nla_get_u32(bss[NL80211_BSS_SEEN_MS_AGO]);
	if (ie_len) {
		res->ie_len = ie_len;
		res->ie = (u8 *) zalloc(ie_len);
		if (!res->ie) {
			free_scan_result(res);
			goto done;
		}
		memcpy(res->ie, ie, ie_len);
	}
	if (beacon_ie_len) {
		res->beacon_ie_len = beacon_ie_len;
		res->beacon_ie = (u8 *) zalloc(beacon_ie_len);
		if (!res->beacon_ie) {
			free_scan_result(res);
			goto done;
		}
		memcpy(res->beacon_ie, beacon_ie, beacon_ie_len);
	}
	if (bss[NL80211_BSS_STATUS]) {
		enum nl80211_bss_status status =
			nla_get_u32(bss[NL80211_BSS_STATUS]);
		switch (status) {
		case NL80211_BSS_STATUS_AUTHENTICATED:
			res->flags |= WFS_SCAN_AUTHENTICATED;
			break;
		case NL80211_BSS_STATUS_ASSOCIATED:
			res->flags |= WFS_SCAN_ASSOCIATED;
			break;
		default:
			break;
		}
	}
	if (ie_len || beacon_ie_len) {
		const u8 *ssid = NULL;
		u32 ssid_len = 0;
		if (ie_len)
			ssid = get_ie_ssid(ie, ie_len, &ssid_len);
		else
			ssid = get_ie_ssid(beacon_ie, beacon_ie_len, &ssid_len);
		if (ssid) {
			memset(res->ssid, 0, sizeof(res->ssid));
			memcpy(res->ssid, ssid, ssid_len);
			res->ssid_len = ssid_len;
		}
	}

	tmp = realloc(scan_results->table, (scan_results->num + 1) *
			sizeof(struct wfs_driver_scan_result *));
	if (!tmp) {
		free_scan_result(res);
		goto done;
	}
	tmp[scan_results->num++] = res;
	scan_results->table = tmp;

done:
	return NL_SKIP;
}


static enum wfs_internal_err
nl_get_assoc_freq(struct wfs_iface *cur_if, u32 *freq)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	struct nl80211_bss_info bss_info;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if && freq);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	memset(&bss_info, 0, sizeof(bss_info));

	add_genl_header(msg, info->family_id, NLM_F_DUMP, NL80211_CMD_GET_SCAN);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	bss_info.info = info;
	err = send_and_recv(cur_if, msg, bss_info_handler, &bss_info);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to fetch scan result: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	if (bss_info.assoc_freq)
		*freq = bss_info.assoc_freq;

	wfs_printf(LOG_DEBUG, "Get frequency (%u MHz) of associated AP "
			"successfully", bss_info.assoc_freq);

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


le16 get_fc_from_frame_type(enum wfs_driver_frame_type type)
{
	switch (type) {
	case WFS_DRV_FT_DEFAULT:
		break;
	case WFS_DRV_FT_ACTION:
		return IEEE80211_FC(WLAN_FC_TYPE_MGMT, WLAN_FC_STYPE_ACTION);
	case WFS_DRV_FT_MLME:
		break;
	default:
		wfs_printf(LOG_ERR, "Unsupported frame type: %d", type);
		break;
	}

	return 0;
}


static enum wfs_internal_err
nl_send_frame_core(struct wfs_driver_frame_params *params, u64 *cookie_out)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct wfs_iface *cur_if = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	u8 *buf = NULL;
	u32 buf_len = 0;
	int err = 0;
	u64 cookie = 0;
	struct ieee80211_hdr *hdr = NULL;

	assert(params);

	ENTER();

	cur_if = params->iface;
	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	wfs_printf(LOG_DEBUG, "  Frame: freq=%u duration=%u no_cck=%d"
		"no_wait_ack=%d offchan_ok=%d", params->freq, params->duration,
		params->no_cck, params->no_wait_ack, params->offchan_ok);

	buf_len = (params->is_header_needed) ?
			IEEE80211_HDRLEN + params->data_len : params->data_len;
	buf = (u8 *) zalloc(buf_len);
	if (!buf) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}
	memcpy(buf + ((params->is_header_needed) ? IEEE80211_HDRLEN : 0),
			params->data, params->data_len);
	if (params->is_header_needed) {
		hdr = (struct ieee80211_hdr *) buf;
		hdr->frame_control = get_fc_from_frame_type(params->frame_type);
		memcpy(hdr->addr1, params->dst, ETH_ALEN);
		memcpy(hdr->addr2, params->src, ETH_ALEN);
		memcpy(hdr->addr3, params->bssid, ETH_ALEN);
	}

	wfs_hexdump(LOG_DEBUG, "FRAME", params->data, params->data_len);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_FRAME);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	if (params->freq)
		NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, params->freq);
	if (params->duration)
		NLA_PUT_U32(msg, NL80211_ATTR_DURATION, params->duration);
	if (params->offchan_ok &&
			(info->capa.flags & DRIVER_CAPA_FLAGS_OFFCHANNEL_TX))
		NLA_PUT_FLAG(msg, NL80211_ATTR_OFFCHANNEL_TX_OK);
	if (params->no_cck)
		NLA_PUT_FLAG(msg, NL80211_ATTR_TX_NO_CCK_RATE);
	if (params->no_wait_ack)
		NLA_PUT_FLAG(msg, NL80211_ATTR_DONT_WAIT_FOR_ACK);

	NLA_PUT(msg, NL80211_ATTR_FRAME, params->data_len, params->data);

	err = send_and_recv(cur_if, msg, cookie_handler, &cookie);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Frame command failed: err=%d (%s)",
			"(freq=%u duration=%u)", err, strerror(-err),
			params->freq, params->duration);
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Frame TX request accepted%s; cookie 0x%llx",
		params->no_wait_ack ? " (no wait ACK)" : "",
		(long long unsigned int) cookie);

	if (cookie_out)
		*cookie_out = params->no_wait_ack ? (u64) -1 : cookie;

nla_put_failure:
	FREE(buf);
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static char *frame_type_to_str(enum wfs_driver_frame_type type)
{
	switch (type) {
	case WFS_DRV_FT_DEFAULT:
		return "";
	case WFS_DRV_FT_ACTION:
		return "Action";
	case WFS_DRV_FT_MLME:
		return "MLME";
	default:
		break;
	}

	return "Unknown";
}


static enum wfs_internal_err
nl_assoc_common(struct nl_msg *msg, struct wfs_driver_assoc_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_OS;
	struct nl80211_driver_info *info = NULL;
	struct wfs_iface *cur_if = NULL;

	assert(msg && params);

	cur_if = params->iface;
	info = cur_if->driver_info;

	if (!is_zero_mac_addr(params->bssid)) {
		wfs_printf(LOG_DEBUG, "  * bssid=" MACSTR,
				MAC2STR(params->bssid));
		NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, params->bssid);
	}

	if (params->freq) {
		wfs_printf(LOG_DEBUG, "  * freq=%d", params->freq);
		NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, params->freq);
		info->cur_status.freq = params->freq;
	} else {
		info->cur_status.freq = 0;
	}

	if (params->bg_scan_period >= 0) {
		wfs_printf(LOG_DEBUG, "  * bg_scan_period=%d",
				params->bg_scan_period);
		NLA_PUT_U16(msg, NL80211_ATTR_BG_SCAN_PERIOD,
				params->bg_scan_period);
	}

	if (params->ssid_len) {
		wfs_hexdump_ascii(LOG_DEBUG, "  * SSID", params->ssid,
				params->ssid_len);
		if (params->ssid_len > MAX_SSID_LEN) {
			wfs_printf(LOG_ERR, "Invalid length of SSID: %lu",
					params->ssid_len);
			ret = WFS_INTERNAL_ERR_INVAL;
			goto nla_put_failure;
		}
		NLA_PUT(msg, NL80211_ATTR_SSID, params->ssid_len, params->ssid);
		memcpy(info->cur_status.ssid, params->ssid, params->ssid_len);
		info->cur_status.ssid_len = params->ssid_len;
	}

	wfs_hexdump(LOG_DEBUG, "  * IEs", params->ie, params->ie_len);
	if (params->ie)
		NLA_PUT(msg, NL80211_ATTR_IE, params->ie_len, params->ie);

	if (params->pairwise_suite) {
		int cipher = get_cipher_suite(params->pairwise_suite);
		if (cipher == 0) {
			ret = WFS_INTERNAL_ERR_INVAL;
			goto nla_put_failure;
		}
		wfs_printf(LOG_DEBUG, "  * pairwise=0x%x", cipher);
		NLA_PUT_U32(msg, NL80211_ATTR_CIPHER_SUITES_PAIRWISE, cipher);
	}

	if (params->group_suite) {
		int cipher = get_cipher_suite(params->group_suite);
		if (cipher == 0) {
			ret = WFS_INTERNAL_ERR_INVAL;
			goto nla_put_failure;
		}
		wfs_printf(LOG_DEBUG, "  * group=0x%x", cipher);
		NLA_PUT_U32(msg, NL80211_ATTR_CIPHER_SUITE_GROUP, cipher);
	}

	if (params->key_mgmt_suite == KEY_MGMT_CCKM
			|| params->key_mgmt_suite == KEY_MGMT_IEEE8021X
			|| params->key_mgmt_suite == KEY_MGMT_IEEE8021X_FT
			|| params->key_mgmt_suite == KEY_MGMT_PSK_FT
			|| params->key_mgmt_suite == KEY_MGMT_PSK) {
		int mgmt = get_akm_suite(params->key_mgmt_suite);
		if (mgmt == -1) {
			ret = WFS_INTERNAL_ERR_INVAL;
			goto nla_put_failure;
		}
		NLA_PUT_U32(msg, NL80211_ATTR_AKM_SUITES, mgmt);
	}

	NLA_PUT_FLAG(msg, NL80211_ATTR_CONTROL_PORT);

	if (params->mfp_op == MGMT_FRAME_PROTECTION_REQUIRED)
		NLA_PUT_U32(msg, NL80211_ATTR_USE_MFP, NL80211_MFP_REQUIRED);

	if (params->disable_ht) {
		wfs_printf(LOG_DEBUG, "  * HT disabled");
		NLA_PUT_FLAG(msg, NL80211_ATTR_DISABLE_HT);
	}

	if (params->ht_caps && params->ht_caps_mask) {
		int sz = sizeof(struct ieee80211_ht_capabilities);
		NLA_PUT(msg, NL80211_ATTR_HT_CAPABILITY, sz, params->ht_caps);
		NLA_PUT(msg, NL80211_ATTR_HT_CAPABILITY_MASK, sz,
			params->ht_caps_mask);
	}

#ifdef CONFIG_IEEE80211AC
	if (params->disable_vht) {
		wfs_printf(LOG_DEBUG, "  * VHT disabled");
		NLA_PUT_FLAG(msg, NL80211_ATTR_DISABLE_VHT);
	}

	if (params->vht_caps && params->vht_caps_mask) {
		int sz = sizeof(struct ieee80211_vht_capabilities);
		NLA_PUT(msg, NL80211_ATTR_VHT_CAPABILITY, sz, params->vht_caps);
		NLA_PUT(msg, NL80211_ATTR_VHT_CAPABILITY_MASK, sz,
			params->vht_caps_mask);
	}
#endif
	ret = WFS_INTERNAL_ERR_SUCCESS;

nla_put_failure:
	return ret;
}


static int get_country_handler(struct nl_msg *msg, void *arg)
{
	u8 *country_code = arg;
	struct nlattr *tb_msg[NL80211_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));

	nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
		  genlmsg_attrlen(gnlh, 0), NULL);
	if (!tb_msg[NL80211_ATTR_REG_ALPHA2]) {
		wfs_printf(LOG_DEBUG, "nl80211: No country information available");
		return NL_SKIP;
	}
	memcpy(country_code, nla_data(tb_msg[NL80211_ATTR_REG_ALPHA2]), 2);
	country_code[2] = '\0';

	return NL_SKIP;

}


static u32 sta_flags_to_nl80211(enum wfs_driver_sta_flags flags)
{
	u32 ret = 0;

	if (flags & WFS_DRV_STA_AUTHORIZED)
		ret |= BIT(NL80211_STA_FLAG_AUTHORIZED);
	if (flags & WFS_DRV_STA_SHORT_PREAMBLE)
		ret |= BIT(NL80211_STA_FLAG_SHORT_PREAMBLE);
	if (flags & WFS_DRV_STA_WME)
		ret |= BIT(NL80211_STA_FLAG_WME);
	if (flags & WFS_DRV_STA_MFP)
		ret |= BIT(NL80211_STA_FLAG_MFP);
	if (flags & WFS_DRV_STA_AUTHENTICATED)
		ret |= BIT(NL80211_STA_FLAG_AUTHENTICATED);
	if (flags & WFS_DRV_STA_TDLS_PEER)
		ret |= BIT(NL80211_STA_FLAG_TDLS_PEER);
	if (flags & WFS_DRV_STA_ASSOCIATED)
		ret |= BIT(NL80211_STA_FLAG_ASSOCIATED);

	return ret;
}


static enum wfs_internal_err
nl_set_sta_flags2(struct nl_msg *msg, enum wfs_driver_sta_flags mask,
		enum wfs_driver_sta_flags set)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_OS;
	struct nl80211_sta_flag_update upd;

	assert(msg);

	memset(&upd, 0, sizeof(upd));

	upd.mask = sta_flags_to_nl80211(mask);
	upd.set = sta_flags_to_nl80211(set);
	wfs_printf(LOG_DEBUG, "flags set=0x%x mask=0x%x", upd.set, upd.mask);

	NLA_PUT(msg, NL80211_ATTR_STA_FLAGS2, sizeof(upd), &upd);

	ret = WFS_INTERNAL_ERR_SUCCESS;

nla_put_failure:
	return ret;
}


static enum wfs_internal_err
nl80211_del_sta(struct wfs_iface *cur_if, const u8 *addr);

static enum wfs_internal_err
nl80211_send_frame(struct wfs_driver_frame_params *params);

static enum wfs_internal_err
nl_sta_disconnect(struct wfs_iface *cur_if, const u8 *addr, u16 reason_code,
		u16 stype)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct ieee80211_mgmt mgmt;
	struct wfs_driver_frame_params params;

	assert(cur_if && addr);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto done;

	if (info->capa.device_ap_sme) {
		ret = nl80211_del_sta(cur_if, addr);
		goto done;
	}

	memset(&mgmt, 0, sizeof(mgmt));
	mgmt.frame_control =
			IEEE80211_FC(WLAN_FC_TYPE_MGMT, stype);
	memcpy(mgmt.da, addr, ETH_ALEN);
	memcpy(mgmt.sa, info->cur_status.bssid, ETH_ALEN);
	memcpy(mgmt.bssid, info->cur_status.bssid, ETH_ALEN);
	mgmt.u.deauth.reason_code = host_to_le16(reason_code);

	memset(&params, 0, sizeof(params));
	params.iface = cur_if;
	params.frame_type = WFS_DRV_FT_MLME;
	params.data = (u8 *) &mgmt;
	params.data_len = IEEE80211_HDRLEN + sizeof(mgmt.u.deauth);
	params.freq = info->cur_status.freq;
	ret = nl80211_send_frame(&params);

done:
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl_set_bss(struct wfs_driver_ap_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct wfs_iface *cur_if = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(params);

	ENTER();

	cur_if = params->iface;
	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	wfs_printf(LOG_DEBUG, "Set BSS (if_idx=%d)", if_idx);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_SET_BSS);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	if (params->cts_protection_enabled >= 0)
		NLA_PUT_U8(msg, NL80211_ATTR_BSS_CTS_PROT,
				params->cts_protection_enabled);
	if (params->preamble_enabled >= 0)
		NLA_PUT_U8(msg, NL80211_ATTR_BSS_SHORT_PREAMBLE,
				params->preamble_enabled);
	if (params->sst_enabled >= 0)
		NLA_PUT_U8(msg, NL80211_ATTR_BSS_SHORT_SLOT_TIME,
				params->sst_enabled);
	if (params->ht_opmode >= 0)
		NLA_PUT_U16(msg, NL80211_ATTR_BSS_HT_OPMODE,
				params->ht_opmode);
	if (params->need_isolation >= 0)
		NLA_PUT_U8(msg, NL80211_ATTR_AP_ISOLATE,
				params->need_isolation);

	if (params->basic_rates && params->basic_rates_len) {
		u8 rates[NL80211_MAX_SUPP_RATES];
		u8 rates_len = 0, min_len = 0;
		int i = 0;

		memset(rates, 0, sizeof(rates));
		min_len = MIN(params->basic_rates_len,
				(u32) NL80211_MAX_SUPP_RATES);
		for (i = 0; i < min_len; ++i)
			rates[rates_len++] = params->basic_rates[i] / 5;

		NLA_PUT(msg, NL80211_ATTR_BSS_BASIC_RATES, rates_len, rates);
	}

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to set BSS: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send set_bss request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl_del_beacon(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	wfs_printf(LOG_DEBUG, "Delete Beacon (if_idx=%d)", if_idx);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_DEL_BEACON);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to delete Beacon: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send del_beacon request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static int get_iface_info_handler(struct nl_msg *msg, void *arg)
{
	struct nlattr *tb[NL80211_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nl80211_get_params *params = arg;

	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
			genlmsg_attrlen(gnlh, 0), NULL);

	if (tb[NL80211_ATTR_WIPHY])
		params->wiphy_idx = nla_get_u32(tb[NL80211_ATTR_WIPHY]);

	if (tb[NL80211_ATTR_IFTYPE])
		params->nl_iftype = nla_get_u32(tb[NL80211_ATTR_IFTYPE]);

	if (params->addr && tb[NL80211_ATTR_MAC])
		memcpy(params->addr, nla_data(tb[NL80211_ATTR_MAC]), ETH_ALEN);

	return NL_SKIP;
}


static enum wfs_internal_err
get_iftype(struct wfs_iface *cur_if, enum nl80211_iftype *nl_iftype)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;
	struct nl80211_get_params params = {
		.nl_iftype = NL80211_IFTYPE_UNSPECIFIED,
		.addr      = NULL,
	};

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	wfs_printf(LOG_DEBUG, "Get iftype (if_idx=%d)", if_idx);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_GET_INTERFACE);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	err = send_and_recv(cur_if, msg, get_iface_info_handler, &params);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to get iftype: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	*nl_iftype = params.nl_iftype;

	wfs_printf(LOG_DEBUG, "Got iftype successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
set_iftype(struct wfs_iface *cur_if, enum nl80211_iftype nl_iftype)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	wfs_printf(LOG_DEBUG, "Set iftype (if_idx=%d) to %d (%s)", if_idx,
			iftype_to_str(nl_iftype));

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_SET_INTERFACE);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	NLA_PUT_U32(msg, NL80211_ATTR_IFTYPE, nl_iftype);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to set iftype: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send set_iftype request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
turn_iface_p2p_dev(struct wfs_iface *cur_if, u8 up)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;
	char *action = (up) ? "Start" : "Stop";

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	wfs_printf(LOG_DEBUG, "%s P2P device (if_idx=%d)", action, if_idx);

	add_genl_header(msg, info->family_id, 0,
			(up) ? NL80211_CMD_START_P2P_DEVICE
				: NL80211_CMD_STOP_P2P_DEVICE);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to %s P2P device: "
				"err=%d (%s)", action, err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send %s P2P device request successfully",
			action);

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
turn_iface(struct wfs_iface *cur_if, u8 up)
{
	struct nl80211_driver_info *info = NULL;

	assert(cur_if);

	info = cur_if->driver_info;

	if (info->cur_status.nl_iftype == NL80211_IFTYPE_P2P_DEVICE)
		return turn_iface_p2p_dev(cur_if, up);

	return (up) ? linux_turn_iface_up(info->ioctl_sock, cur_if->ifname) :
			linux_turn_iface_down(info->ioctl_sock, cur_if->ifname);
}


static enum wfs_internal_err
nl_set_iftype(struct wfs_iface *cur_if, enum nl80211_iftype nl_iftype)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	enum nl80211_iftype cur_nl_iftype = NL80211_IFTYPE_UNSPECIFIED;
	int i = 0, is_down = 0;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;

	wfs_printf(LOG_DEBUG, "Original iftype is %d",
			info->cur_status.nl_iftype);

	/** No more action if already in requested iftype */
	ret = get_iftype(cur_if, &cur_nl_iftype);
	if (ret) {
		goto out;
	} else if (cur_nl_iftype != info->cur_status.nl_iftype) {
		wfs_printf(LOG_DEBUG, "cur_status.nl_iftype %d is out-of-date."
				" Update to %d", info->cur_status.nl_iftype,
				cur_nl_iftype);
		info->cur_status.nl_iftype = cur_nl_iftype;
	}
	if (info->cur_status.nl_iftype == nl_iftype) {
		wfs_printf(LOG_DEBUG, "Interface already in requested iftype");
		goto out;
	}

	ret = set_iftype(cur_if, nl_iftype);
	if (ret == WFS_INTERNAL_ERR_SUCCESS)
		goto out;

	/**
	 * mac80211 doesn't allow iftype changes while the device is up, so
	 * take the device down, try to set the mode again, and bring the
	 * device back up.
	 */
	wfs_printf(LOG_DEBUG, "Try iftype change after setting interface down");

#define TRY_TIMES 10
	for (i = 0; i < TRY_TIMES; ++i) {
		os_sleep(0, 100000);
		if (!is_down) {
			ret = turn_iface(cur_if, 0);
			if (ret == WFS_INTERNAL_ERR_SUCCESS)
				is_down = 1;
			else
				continue;
		}
		/**
		 * Try to set the iftype again while the interface is
		 * down
		 */
		ret = set_iftype(cur_if, nl_iftype);
		if (ret)
			continue;
		ret = turn_iface(cur_if, 1);
		if (ret == WFS_INTERNAL_ERR_SUCCESS)
			break;
	}

out:
	if (ret == WFS_INTERNAL_ERR_SUCCESS) {
		info->cur_status.nl_iftype = nl_iftype;
		wfs_printf(LOG_DEBUG, "Iftype is changed to %d",
				info->cur_status.nl_iftype);
	} else {
		wfs_printf(LOG_DEBUG, "Failed to change iftype from %d to %d",
				info->cur_status.nl_iftype, nl_iftype);
	}

	LEAVE();
	return ret;
}


static int proto_feature_handler(struct nl_msg *msg, void *arg)
{
	u32 *feat = arg;
	struct nlattr *tb_msg[NL80211_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));

	nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
		  genlmsg_attrlen(gnlh, 0), NULL);

	if (tb_msg[NL80211_ATTR_PROTOCOL_FEATURES])
		*feat = nla_get_u32(tb_msg[NL80211_ATTR_PROTOCOL_FEATURES]);

	return NL_SKIP;
}


static enum wfs_internal_err
nl_get_proto_features(struct wfs_iface *cur_if, u32 *feat)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;

	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if && feat);

	ENTER();

	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	wfs_printf(LOG_DEBUG, "Get protocol features (if_idx=%d)", if_idx);

	add_genl_header(msg, info->family_id, 0,
			NL80211_CMD_GET_PROTOCOL_FEATURES);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	err = send_and_recv(cur_if, msg, proto_feature_handler, feat);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to get protocol features: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG,
			"Send get_protocol_features request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static void wiphy_info_freq(struct wfs_driver_hw_info *hw_info,
		struct wfs_driver_chan_info *chan, struct nlattr *tb_freq[])
{
	u8 channel;
	chan->freq = nla_get_u32(tb_freq[NL80211_FREQUENCY_ATTR_FREQ]);
	chan->flags = 0;
	if (freq_to_chan(chan->freq, &channel))
		chan->chan = channel;

	if (tb_freq[NL80211_FREQUENCY_ATTR_DISABLED])
		chan->flags |= WFS_DRV_CHAN_DISABLED;
	if (tb_freq[NL80211_FREQUENCY_ATTR_NO_IR])
		chan->flags |= WFS_DRV_CHAN_PASSIVE_SCAN | WFS_DRV_CHAN_NO_IBSS;
	if (tb_freq[NL80211_FREQUENCY_ATTR_RADAR])
		chan->flags |= WFS_DRV_CHAN_RADAR;

	if (tb_freq[NL80211_FREQUENCY_ATTR_DFS_STATE]) {
		enum nl80211_dfs_state state =
			nla_get_u32(tb_freq[NL80211_FREQUENCY_ATTR_DFS_STATE]);

		switch (state) {
		case NL80211_DFS_USABLE:
			chan->flags |= WFS_DRV_CHAN_DFS_USABLE;
			break;
		case NL80211_DFS_AVAILABLE:
			chan->flags |= WFS_DRV_CHAN_DFS_AVAILABLE;
			break;
		case NL80211_DFS_UNAVAILABLE:
			chan->flags |= WFS_DRV_CHAN_DFS_UNAVAILABLE;
			break;
		}
	}
}


static int wiphy_info_freqs(struct nl80211_wiphy_info *wiphy_info,
		struct wfs_driver_hw_info *hw_info, struct nlattr *tb)
{
	static struct nla_policy freq_policy[NL80211_FREQUENCY_ATTR_MAX + 1] = {
		[NL80211_FREQUENCY_ATTR_FREQ] = { .type = NLA_U32 },
		[NL80211_FREQUENCY_ATTR_DISABLED] = { .type = NLA_FLAG },
		[NL80211_FREQUENCY_ATTR_NO_IR] = { .type = NLA_FLAG },
		[NL80211_FREQUENCY_ATTR_RADAR] = { .type = NLA_FLAG },
		[NL80211_FREQUENCY_ATTR_MAX_TX_POWER] = { .type = NLA_U32 },
		[NL80211_FREQUENCY_ATTR_DFS_STATE] = { .type = NLA_U32 },
	};
	u32 new_channels = 0;
	struct wfs_driver_chan_info *channel = NULL;
	struct nlattr *tb_freq[NL80211_FREQUENCY_ATTR_MAX + 1];
	struct nlattr *nl_freq = NULL;
	int rem_freq = 0, idx = 0;

	if (!tb)
		return NL_OK;

	nla_for_each_nested(nl_freq, tb, rem_freq) {
		nla_parse(tb_freq, NL80211_FREQUENCY_ATTR_MAX,
			nla_data(nl_freq), nla_len(nl_freq), freq_policy);
		if (!tb_freq[NL80211_FREQUENCY_ATTR_FREQ])
			continue;
		++new_channels;
	}

	channel = realloc(hw_info->channels,
			(hw_info->channels_num + new_channels) *
			sizeof(*channel));
	if (!channel)
		return NL_SKIP;

	hw_info->channels = channel;
	hw_info->channels_num += new_channels;

	idx = wiphy_info->last_chan_idx;

	nla_for_each_nested(nl_freq, tb, rem_freq) {
		nla_parse(tb_freq, NL80211_FREQUENCY_ATTR_MAX,
			  nla_data(nl_freq), nla_len(nl_freq), freq_policy);
		if (!tb_freq[NL80211_FREQUENCY_ATTR_FREQ])
			continue;
		wiphy_info_freq(hw_info, &hw_info->channels[idx++], tb_freq);
	}
	wiphy_info->last_chan_idx = idx;

	return NL_OK;
}


static int wiphy_info_rates(struct wfs_driver_hw_info *hw_info,
		struct nlattr *tb)
{
	static struct nla_policy rate_policy[NL80211_BITRATE_ATTR_MAX + 1] = {
		[NL80211_BITRATE_ATTR_RATE] = { .type = NLA_U32 },
		[NL80211_BITRATE_ATTR_2GHZ_SHORTPREAMBLE] =
		{ .type = NLA_FLAG },
	};
	struct nlattr *tb_rate[NL80211_BITRATE_ATTR_MAX + 1];
	struct nlattr *nl_rate = NULL;
	int rem_rate = 0, idx = 0;

	if (!tb)
		return NL_OK;

	nla_for_each_nested(nl_rate, tb, rem_rate) {
		nla_parse(tb_rate, NL80211_BITRATE_ATTR_MAX,
			  nla_data(nl_rate), nla_len(nl_rate),
			  rate_policy);
		if (!tb_rate[NL80211_BITRATE_ATTR_RATE])
			continue;
		++(hw_info->rates_num);
	}

	hw_info->rates = (u32 *) zalloc(hw_info->rates_num * sizeof(u32));
	if (!hw_info->rates)
		return NL_SKIP;

	idx = 0;

	nla_for_each_nested(nl_rate, tb, rem_rate) {
		nla_parse(tb_rate, NL80211_BITRATE_ATTR_MAX,
			  nla_data(nl_rate), nla_len(nl_rate),
			  rate_policy);
		if (!tb_rate[NL80211_BITRATE_ATTR_RATE])
			continue;
		hw_info->rates[idx++] = nla_get_u32(
			tb_rate[NL80211_BITRATE_ATTR_RATE]);
	}

	return NL_OK;
}


static int wiphy_info_band(struct nl80211_wiphy_info *wiphy_info,
		struct nlattr *nl_band)
{
	struct nlattr *tb_band[NL80211_BAND_ATTR_MAX + 1];
	struct wfs_driver_hw_info *hw_info = NULL;
	int ret = NL_OK;

	if (wiphy_info->last_nla_type != nl_band->nla_type) {
		hw_info = realloc(wiphy_info->hw_info_array,
			(wiphy_info->hw_info_num + 1) * sizeof(*hw_info));
		if (!hw_info)
			return NL_SKIP;
		wiphy_info->hw_info_array = hw_info;
		hw_info = &wiphy_info->hw_info_array[wiphy_info->hw_info_num];
		memset(hw_info, 0, sizeof(*hw_info));
		/**
		 * Unsupported VHT MCS stream is defined as value 3, so the VHT
		 * MCS RX/TX map must be initialized with 0xffff to mark all 8
		 * possible streams as unsupported. This will be overridden if
		 * driver advertises VHT support.
		 */
		hw_info->vht_mcs_set[0] = 0xff;
		hw_info->vht_mcs_set[1] = 0xff;
		hw_info->vht_mcs_set[4] = 0xff;
		hw_info->vht_mcs_set[5] = 0xff;
		++(wiphy_info->hw_info_num);
		wiphy_info->last_nla_type = nl_band->nla_type;
		wiphy_info->last_chan_idx = 0;
	} else {
		hw_info =
			&wiphy_info->hw_info_array[wiphy_info->hw_info_num - 1];
	}

	nla_parse(tb_band, NL80211_BAND_ATTR_MAX, nla_data(nl_band),
			nla_len(nl_band), NULL);

	/** HT params */
	if (tb_band[NL80211_BAND_ATTR_HT_CAPA])
		hw_info->ht_caps =
			nla_get_u16(tb_band[NL80211_BAND_ATTR_HT_CAPA]);
	if (tb_band[NL80211_BAND_ATTR_HT_AMPDU_FACTOR])
		hw_info->ampdu_params |=
			nla_get_u8(tb_band[NL80211_BAND_ATTR_HT_AMPDU_FACTOR])
			& 0x03;
	if (tb_band[NL80211_BAND_ATTR_HT_AMPDU_DENSITY])
		hw_info->ampdu_params |=
			nla_get_u8(tb_band[NL80211_BAND_ATTR_HT_AMPDU_DENSITY])
			<< 2;
	if (tb_band[NL80211_BAND_ATTR_HT_MCS_SET]
			&& nla_len(tb_band[NL80211_BAND_ATTR_HT_MCS_SET]) >= 16)
		memcpy(hw_info->ht_mcs_set,
			nla_data(tb_band[NL80211_BAND_ATTR_HT_MCS_SET]), 16);

	/** VHT params */
	if (tb_band[NL80211_BAND_ATTR_VHT_CAPA])
		hw_info->vht_caps =
			nla_get_u32(tb_band[NL80211_BAND_ATTR_VHT_CAPA]);
	if (tb_band[NL80211_BAND_ATTR_VHT_MCS_SET]
			&& nla_len(tb_band[NL80211_BAND_ATTR_VHT_MCS_SET]) >= 8)
		memcpy(hw_info->vht_mcs_set,
			nla_data(tb_band[NL80211_BAND_ATTR_VHT_MCS_SET]), 8);

	/** Frequencies */
	ret = wiphy_info_freqs(wiphy_info, hw_info,
			tb_band[NL80211_BAND_ATTR_FREQS]);
	if (ret != NL_OK)
		return ret;
	/** Rates */
	ret = wiphy_info_rates(hw_info, tb_band[NL80211_BAND_ATTR_RATES]);
	if (ret != NL_OK)
		return ret;

	return NL_OK;
}


static int wiphy_info_handler(struct nl_msg *msg, void *arg)
{
	struct nlattr *tb_msg[NL80211_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nl80211_wiphy_info *wiphy_info = arg;
	struct nlattr *nl_band = NULL;
	int rem_band = 0;

	nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
			genlmsg_attrlen(gnlh, 0), NULL);

	if (!tb_msg[NL80211_ATTR_WIPHY_BANDS])
		return NL_SKIP;

	nla_for_each_nested(nl_band, tb_msg[NL80211_ATTR_WIPHY_BANDS],
			rem_band) {
		int ret = wiphy_info_band(wiphy_info, nl_band);
		if (ret != NL_OK)
			return ret;
	}

	return NL_SKIP;
}


static void set_ht40_chan(struct wfs_driver_hw_info *hw_info, u32 start,
		u32 end)
{
	u32 i = 0;
	for (i = 0; i < hw_info->channels_num; ++i) {
		struct wfs_driver_chan_info *chan = &hw_info->channels[i];
		if (start + 10 <= chan->freq && chan->freq + 10 <= end)
			chan->flags = WFS_DRV_CHAN_HT40;
	}
}


static void reg_rule_ht40(struct nl80211_wiphy_info *wiphy_info, u32 start,
		u32 end)
{
	u32 i = 0;

	for (i = 0; i < wiphy_info->hw_info_num; ++i) {
		if (!(wiphy_info->hw_info_array[i].ht_caps
				& HT_CAP_INFO_SUPP_CHANNEL_WIDTH_SET))
			continue;
		set_ht40_chan(&wiphy_info->hw_info_array[i], start, end);
	}
}


static void reg_rule_max_eirp(struct nl80211_wiphy_info *wiphy_info, u32 start,
		u32 end, u32 max_eirp)
{
	u32 i = 0;

	for (i = 0; i < wiphy_info->hw_info_num; ++i) {
		u32 j = 0;
		struct wfs_driver_hw_info *hw_info =
			&wiphy_info->hw_info_array[i];
		for (j = 0; j < hw_info->channels_num; ++j) {
			struct wfs_driver_chan_info *chan =
				&hw_info->channels[j];
			if (start + 10 <= chan->freq && chan->freq + 10 <= end)
				chan->max_tx_power = max_eirp;
		}
	}
}


static void set_ht40_sec_chan(struct wfs_driver_hw_info *hw_info, u32 start,
		u32 end)
{
	u32 i = 0;

	for (i = 0; i < hw_info->channels_num; ++i) {
		struct wfs_driver_chan_info *chan = &hw_info->channels[i];
		if (!(chan->flags & WFS_DRV_CHAN_HT40))
			continue;
		if (start + 30 <= chan->freq && chan->freq <= end + 10)
			chan->flags = WFS_DRV_CHAN_HT40MINUS;
		if (start <= chan->freq + 10 && chan->freq + 30 <= end)
			chan->flags = WFS_DRV_CHAN_HT40PLUS;
	}
}


static void reg_rule_sec(struct nlattr *tb[],
		struct nl80211_wiphy_info *wiphy_info)
{
	u32 start = 0, end = 0, max_bw = 0, i = 0;

	if (tb[NL80211_ATTR_FREQ_RANGE_START] == NULL ||
			tb[NL80211_ATTR_FREQ_RANGE_END] == NULL ||
			tb[NL80211_ATTR_FREQ_RANGE_MAX_BW] == NULL)
		return;

	start = nla_get_u32(tb[NL80211_ATTR_FREQ_RANGE_START]) / 1000;
	end = nla_get_u32(tb[NL80211_ATTR_FREQ_RANGE_END]) / 1000;
	max_bw = nla_get_u32(tb[NL80211_ATTR_FREQ_RANGE_MAX_BW]) / 1000;

	if (max_bw < 20)
		return;

	for (i = 0; i < wiphy_info->hw_info_num; ++i) {
		if (!(wiphy_info->hw_info_array[i].ht_caps
				& HT_CAP_INFO_SUPP_CHANNEL_WIDTH_SET))
			continue;
		set_ht40_sec_chan(&wiphy_info->hw_info_array[i], start, end);
	}
}


static void set_vht_chan(struct wfs_driver_hw_info *hw_info, u32 start, u32 end)
{
	u32 i = 0;

	for (i = 0; i < hw_info->channels_num; ++i) {
		struct wfs_driver_chan_info *chan = &hw_info->channels[i];
		if (start + 10 <= chan->freq && chan->freq + 70 <= end)
			chan->flags = WFS_DRV_CHAN_VHT_10_70;
		if (start + 30 <= chan->freq && chan->freq + 50 <= end)
			chan->flags = WFS_DRV_CHAN_VHT_30_50;
		if (start + 50 <= chan->freq && chan->freq + 30 <= end)
			chan->flags = WFS_DRV_CHAN_VHT_50_30;
		if (start + 70 <= chan->freq && chan->freq + 10 <= end)
			chan->flags = WFS_DRV_CHAN_VHT_70_10;
	}
}


static void reg_rule_vht(struct nlattr *tb[],
		struct nl80211_wiphy_info *wiphy_info)
{
	u32 start = 0, end = 0, max_bw = 0, i = 0;

	if (tb[NL80211_ATTR_FREQ_RANGE_START] == NULL ||
			tb[NL80211_ATTR_FREQ_RANGE_END] == NULL ||
			tb[NL80211_ATTR_FREQ_RANGE_MAX_BW] == NULL)
		return;

	start = nla_get_u32(tb[NL80211_ATTR_FREQ_RANGE_START]) / 1000;
	end = nla_get_u32(tb[NL80211_ATTR_FREQ_RANGE_END]) / 1000;
	max_bw = nla_get_u32(tb[NL80211_ATTR_FREQ_RANGE_MAX_BW]) / 1000;

	if (max_bw < 80)
		return;

	for (i = 0; i < wiphy_info->hw_info_num; ++i) {
		if (!(wiphy_info->hw_info_array[i].ht_caps
				& HT_CAP_INFO_SUPP_CHANNEL_WIDTH_SET))
			continue;
		if (!wiphy_info->hw_info_array[i].vht_caps)
			continue;
		set_vht_chan(&wiphy_info->hw_info_array[i], start, end);
	}
}


static int get_reg_handler(struct nl_msg *msg, void *arg)
{
	struct nl80211_wiphy_info *wiphy_info = arg;
	struct nlattr *tb_msg[NL80211_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nlattr *nl_rule = NULL;
	struct nlattr *tb_rule[NL80211_FREQUENCY_ATTR_MAX + 1];
	int rem_rule = 0;
	static struct nla_policy reg_policy[NL80211_FREQUENCY_ATTR_MAX + 1] = {
		[NL80211_ATTR_REG_RULE_FLAGS] = { .type = NLA_U32 },
		[NL80211_ATTR_FREQ_RANGE_START] = { .type = NLA_U32 },
		[NL80211_ATTR_FREQ_RANGE_END] = { .type = NLA_U32 },
		[NL80211_ATTR_FREQ_RANGE_MAX_BW] = { .type = NLA_U32 },
		[NL80211_ATTR_POWER_RULE_MAX_ANT_GAIN] = { .type = NLA_U32 },
		[NL80211_ATTR_POWER_RULE_MAX_EIRP] = { .type = NLA_U32 },
	};

	nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
			genlmsg_attrlen(gnlh, 0), NULL);
	if (!tb_msg[NL80211_ATTR_REG_ALPHA2] ||
			!tb_msg[NL80211_ATTR_REG_RULES]) {
		wfs_printf(LOG_DEBUG, "No regulatory information available");
		return NL_SKIP;
	}

	wfs_printf(LOG_DEBUG, "Regulatory information - country=%s",
			(char *) nla_data(tb_msg[NL80211_ATTR_REG_ALPHA2]));

	nla_for_each_nested(nl_rule, tb_msg[NL80211_ATTR_REG_RULES], rem_rule) {
		u32 start = 0, end = 0, max_eirp = 0, max_bw = 0;
		nla_parse(tb_rule, NL80211_FREQUENCY_ATTR_MAX,
			nla_data(nl_rule), nla_len(nl_rule), reg_policy);
		if (tb_rule[NL80211_ATTR_FREQ_RANGE_START] == NULL ||
				tb_rule[NL80211_ATTR_FREQ_RANGE_END] == NULL)
			continue;
		start = nla_get_u32(tb_rule[NL80211_ATTR_FREQ_RANGE_START])
			/ 1000;
		end = nla_get_u32(tb_rule[NL80211_ATTR_FREQ_RANGE_END]) / 1000;
		if (tb_rule[NL80211_ATTR_POWER_RULE_MAX_EIRP])
			max_eirp =
				nla_get_u32(tb_rule[NL80211_ATTR_POWER_RULE_MAX_EIRP])
				/ 100;
		if (tb_rule[NL80211_ATTR_FREQ_RANGE_MAX_BW])
			max_bw = nla_get_u32(tb_rule[NL80211_ATTR_FREQ_RANGE_MAX_BW])
				/ 1000;

		wfs_printf(LOG_DEBUG, "%u-%u @ %u MHz %u mBm", start, end,
				max_bw, max_eirp);
		if (max_bw >= 40)
			reg_rule_ht40(wiphy_info, start, end);
		if (tb_rule[NL80211_ATTR_POWER_RULE_MAX_EIRP])
			reg_rule_max_eirp(wiphy_info, start, end, max_eirp);
	}

	nla_for_each_nested(nl_rule, tb_msg[NL80211_ATTR_REG_RULES], rem_rule) {
		nla_parse(tb_rule, NL80211_FREQUENCY_ATTR_MAX,
			nla_data(nl_rule), nla_len(nl_rule), reg_policy);
		reg_rule_sec(tb_rule, wiphy_info);
	}

	nla_for_each_nested(nl_rule, tb_msg[NL80211_ATTR_REG_RULES], rem_rule) {
		nla_parse(tb_rule, NL80211_FREQUENCY_ATTR_MAX,
			nla_data(nl_rule), nla_len(nl_rule), reg_policy);
		reg_rule_vht(tb_rule, wiphy_info);
	}

	return NL_SKIP;
}


static enum wfs_internal_err
nl_set_regulatory_flags(struct wfs_iface *cur_if,
		struct nl80211_wiphy_info *wiphy_info)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if && wiphy_info);

	ENTER();

	info = cur_if->driver_info;
	if_idx = if_nametoindex(cur_if->ifname);

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Set regulatory flags (if_idx=%d)", if_idx);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_GET_REG);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	err = send_and_recv(cur_if, msg, get_reg_handler, wiphy_info);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to set regulatory flags: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Set regulatory flags successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
post_process_hw_info(struct wfs_iface *cur_if,
		struct nl80211_wiphy_info *wiphy_info)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct wfs_driver_hw_info *hw_info = NULL, *g_hw_info = NULL;
	u32 i = 0;
	int g_idx = -1;

	info = cur_if->driver_info;

	/** Heuristic to set up HW mode */
	for (i = 0; i < wiphy_info->hw_info_num; ++i) {
		hw_info = &wiphy_info->hw_info_array[i];
		if (hw_info->channels_num == 0)
			continue;
		if (hw_info->channels[0].freq < 4000) {
			u32 j = 0;
			hw_info->mode = WFS_DRV_HW_MODE_IEEE80211B;
			for (j = 0; j < hw_info->rates_num; ++j) {
				if (hw_info->rates[j] > 200) {
					hw_info->mode =
						WFS_DRV_HW_MODE_IEEE80211G;
					break;
				}
			}
		} else if (hw_info->channels[0].freq > 50000) {
			hw_info->mode = WFS_DRV_HW_MODE_IEEE80211AD;
		} else {
			hw_info->mode = WFS_DRV_HW_MODE_IEEE80211A;
		}
	}

	/**
	 * If only 802.11g mode is included, use it to construct matching
	 * 802.11b mode data.
	 */
	for (i = 0; i < wiphy_info->hw_info_num; ++i) {
		hw_info = &wiphy_info->hw_info_array[i];
		if (hw_info->mode == WFS_DRV_HW_MODE_IEEE80211B)
			goto done; /* 802.11b already included */
		if (hw_info->mode == WFS_DRV_HW_MODE_IEEE80211G)
			g_idx = i;
	}

	if (g_idx < 0)
		goto done; /* 2.4GHz band not supported at all */

	info->cur_status.hw.hw_info = realloc(wiphy_info->hw_info_array,
			(wiphy_info->hw_info_num + 1) * sizeof(*hw_info));
	if (!info->cur_status.hw.hw_info) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto done;
	}
	info->cur_status.hw.hw_info_num = wiphy_info->hw_info_num;

	g_hw_info = &info->cur_status.hw.hw_info[g_idx];
	hw_info = &info->cur_status.hw.hw_info[wiphy_info->hw_info_num];
	memset(hw_info, 0, sizeof(*hw_info));

	/** HW mode */
	hw_info->mode = WFS_DRV_HW_MODE_IEEE80211B;

	/** Copy channels info from 802.11g entry */
	hw_info->channels_num = g_hw_info->channels_num;
	hw_info->channels = (struct wfs_driver_chan_info *)
			zalloc(hw_info->channels_num *
					sizeof(struct wfs_driver_chan_info));
	if (!hw_info->channels) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto fail;
	}
	memcpy(hw_info->channels, g_hw_info->channels,
		hw_info->channels_num * sizeof(struct wfs_driver_chan_info));

	/** Fetch 802.11b rates info */
	hw_info->rates_num = 0;
	hw_info->rates = (u32 *) zalloc(4 * sizeof(u32));
	if (!hw_info->rates) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto fail;
	}
	for (i = 0; i < g_hw_info->channels_num; ++i) {
		u32 rate = g_hw_info->rates[i];
		if (rate != 10 && rate != 20 && rate != 55 && rate != 110)
			continue;
		hw_info->rates[hw_info->rates_num++] = rate;
		if (hw_info->rates_num == 4)
			break;
	}
	if (hw_info->rates_num == 0)
		goto fail;

	/** New 802.11b entry is added */
	info->cur_status.hw.hw_info_num = wiphy_info->hw_info_num + 1;

	wfs_printf(LOG_DEBUG, "Added 802.11b mode based on 802.11g "
			"information");
	goto done;
fail:
	FREE(hw_info->channels);
	FREE(hw_info->rates);
	FREE(hw_info);
done:
	return ret;
}


static void capa_info_supported_iftypes(struct wfs_driver_capa *capa,
		struct nlattr *tb)
{
	struct nlattr *nl_iftype = NULL;
	int rem_iftype = 0;
	u8 p2p_go_supported = 0, p2p_client_supported = 0;

	if (!tb)
		return;

	nla_for_each_nested(nl_iftype, tb, rem_iftype) {
		switch (nla_type(nl_iftype)) {
		case NL80211_IFTYPE_AP:
			capa->flags |= DRIVER_CAPA_FLAGS_AP;
			break;
		case NL80211_IFTYPE_ADHOC:
			capa->flags |= DRIVER_CAPA_FLAGS_IBSS;
			break;
		case NL80211_IFTYPE_P2P_DEVICE:
			capa->flags |= DRIVER_CAPA_FLAGS_DEDICATED_P2P_DEVICE;
			break;
		case NL80211_IFTYPE_P2P_GO:
			p2p_go_supported = 1;
			break;
		case NL80211_IFTYPE_P2P_CLIENT:
			p2p_client_supported = 1;
			break;
		case NL80211_IFTYPE_MONITOR:
			capa->monitor = 1;
			break;
		default:
			break;
		}
	}

	if (p2p_go_supported && p2p_client_supported)
		capa->flags |= DRIVER_CAPA_FLAGS_P2P_CAPABLE;
}


static enum wfs_internal_err
capa_info_iface_comb_process(struct wfs_driver_capa *capa,
		struct nlattr *nl_comb)
{
	struct nlattr *tb_comb[NUM_NL80211_IFACE_COMB];
	struct nlattr *tb_limit[NUM_NL80211_IFACE_LIMIT];
	struct nlattr *nl_limit, *nl_iftype;
	int err = 0, rem_limit = 0, rem_iftype = 0;
	int combination_has_p2p = 0, combination_has_mgd = 0;
	u32 num_concur_chans = 0;
	static struct nla_policy
	iface_combination_policy[NUM_NL80211_IFACE_COMB] = {
		[NL80211_IFACE_COMB_LIMITS] = { .type = NLA_NESTED },
		[NL80211_IFACE_COMB_MAXNUM] = { .type = NLA_U32 },
		[NL80211_IFACE_COMB_STA_AP_BI_MATCH] = { .type = NLA_FLAG },
		[NL80211_IFACE_COMB_NUM_CHANNELS] = { .type = NLA_U32 },
		[NL80211_IFACE_COMB_RADAR_DETECT_WIDTHS] = { .type = NLA_U32 },
	},
	iface_limit_policy[NUM_NL80211_IFACE_LIMIT] = {
		[NL80211_IFACE_LIMIT_TYPES] = { .type = NLA_NESTED },
		[NL80211_IFACE_LIMIT_MAX] = { .type = NLA_U32 },
	};

	err = nla_parse_nested(tb_comb, MAX_NL80211_IFACE_COMB, nl_comb,
			iface_combination_policy);
	if (err || !tb_comb[NL80211_IFACE_COMB_LIMITS]
			|| !tb_comb[NL80211_IFACE_COMB_MAXNUM]
			|| !tb_comb[NL80211_IFACE_COMB_NUM_CHANNELS])
		return WFS_INTERNAL_ERR_OS; /* broken combination */

	if (tb_comb[NL80211_IFACE_COMB_RADAR_DETECT_WIDTHS])
		capa->flags |= DRIVER_CAPA_FLAGS_RADAR;

	nla_for_each_nested(nl_limit, tb_comb[NL80211_IFACE_COMB_LIMITS],
			rem_limit) {
		err = nla_parse_nested(tb_limit, MAX_NL80211_IFACE_LIMIT,
				nl_limit, iface_limit_policy);
		if (err || !tb_limit[NL80211_IFACE_LIMIT_TYPES])
			return WFS_INTERNAL_ERR_OS; /* broken combination */

		nla_for_each_nested(nl_iftype,
				tb_limit[NL80211_IFACE_LIMIT_TYPES],
				rem_iftype) {
			int ift = nla_type(nl_iftype);
			if (ift == NL80211_IFTYPE_P2P_GO
					|| ift == NL80211_IFTYPE_P2P_CLIENT)
				combination_has_p2p = 1;
			if (ift == NL80211_IFTYPE_STATION)
				combination_has_mgd = 1;
		}
		if (combination_has_p2p && combination_has_mgd)
			break;
	}

	if (combination_has_p2p && combination_has_mgd) {
		capa->flags |= DRIVER_CAPA_FLAGS_P2P_CONCURRENT
				| DRIVER_CAPA_FLAGS_P2P_MGMT_AND_NON_P2P;
		num_concur_chans =
			nla_get_u32(tb_comb[NL80211_IFACE_COMB_NUM_CHANNELS]);
		if (num_concur_chans > 1)
			capa->num_concur_chans = num_concur_chans;
		return WFS_INTERNAL_ERR_SUCCESS;
	}

	return WFS_INTERNAL_ERR_INVAL;
}


static void capa_info_iface_comb(struct wfs_driver_capa *capa,
		struct nlattr *tb)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nlattr *nl_comb = NULL;
	int rem_comb = 0;

	if (!tb)
		return;

	nla_for_each_nested(nl_comb, tb, rem_comb) {
		ret = capa_info_iface_comb_process(capa, nl_comb);
		if (ret == WFS_INTERNAL_ERR_SUCCESS)
			break;
	}
}


static void capa_info_supported_cmds(struct wfs_driver_capa *capa,
		struct nlattr *tb)
{
	struct nlattr *nl_cmd = NULL;
	int rem_cmd = 0;

	if (!tb)
		return;

	nla_for_each_nested(nl_cmd, tb, rem_cmd) {
		switch (nla_get_u32(nl_cmd)) {
		case NL80211_CMD_AUTHENTICATE:
			capa->flags |= DRIVER_CAPA_FLAGS_SME;
			capa->connect = 1;
			break;
		case NL80211_CMD_CONNECT:
			capa->connect = 1;
			break;
		case NL80211_CMD_START_SCHED_SCAN:
			capa->sched_scan = 1;
			break;
		case NL80211_CMD_PROBE_CLIENT:
			capa->probe_client = 1;
			break;
		case NL80211_CMD_CHANNEL_SWITCH:
			capa->flags |= DRIVER_CAPA_FLAGS_AP_CSA;
			break;
		case NL80211_CMD_SET_QOS_MAP:
			capa->flags |= DRIVER_CAPA_FLAGS_QOS_MAPPING;
			break;
		default:
			break;
		}
	}
}


static void capa_info_cipher_suites(struct wfs_driver_capa *capa,
		struct nlattr *tb)
{
	u32 num = 0, i = 0;
	u32 *ciphers = NULL;

	if (tb == NULL)
		return;

	num = nla_len(tb) / sizeof(u32);
	ciphers = nla_data(tb);
	for (i = 0; i < num; ++i) {
		u32 cipher = ciphers[i];
		wfs_printf(LOG_DEBUG, "Supported cipher %02x-%02x-%02x:%d",
				cipher >> 24, (cipher >> 16) & 0xff,
				(cipher >> 8) & 0xff, cipher & 0xff);
		switch (cipher) {
		case WLAN_CIPHER_SUITE_CCMP_256:
			capa->enc |= DRIVER_CAPA_ENC_CCMP_256;
			break;
		case WLAN_CIPHER_SUITE_GCMP_256:
			capa->enc |= DRIVER_CAPA_ENC_GCMP_256;
			break;
		case WLAN_CIPHER_SUITE_CCMP:
			capa->enc |= DRIVER_CAPA_ENC_CCMP;
			break;
		case WLAN_CIPHER_SUITE_GCMP:
			capa->enc |= DRIVER_CAPA_ENC_GCMP;
			break;
		case WLAN_CIPHER_SUITE_TKIP:
			capa->enc |= DRIVER_CAPA_ENC_TKIP;
			break;
		case WLAN_CIPHER_SUITE_WEP104:
			capa->enc |= DRIVER_CAPA_ENC_WEP104;
			break;
		case WLAN_CIPHER_SUITE_WEP40:
			capa->enc |= DRIVER_CAPA_ENC_WEP40;
			break;
		case WLAN_CIPHER_SUITE_AES_CMAC:
			capa->enc |= DRIVER_CAPA_ENC_BIP;
			break;
		case WLAN_CIPHER_SUITE_BIP_GMAC_128:
			capa->enc |= DRIVER_CAPA_ENC_BIP_GMAC_128;
			break;
		case WLAN_CIPHER_SUITE_BIP_GMAC_256:
			capa->enc |= DRIVER_CAPA_ENC_BIP_GMAC_256;
			break;
		case WLAN_CIPHER_SUITE_BIP_CMAC_256:
			capa->enc |= DRIVER_CAPA_ENC_BIP_CMAC_256;
			break;
		default:
			break;
		}
	}
}


static void capa_info_feature_flags(struct wfs_driver_capa *capa,
		struct nlattr *tb)
{
	u32 flags = 0;

	if (tb == NULL)
		return;

	flags = nla_get_u32(tb);

	if (flags & NL80211_FEATURE_SK_TX_STATUS)
		capa->sk_tx_status = 1;

	if (flags & NL80211_FEATURE_INACTIVITY_TIMER)
		capa->flags |= DRIVER_CAPA_FLAGS_INACTIVITY_TIMER;

	if (flags & NL80211_FEATURE_SAE)
		capa->flags |= DRIVER_CAPA_FLAGS_SAE;

	if (flags & NL80211_FEATURE_NEED_OBSS_SCAN)
		capa->flags |= DRIVER_CAPA_FLAGS_OBSS_SCAN;
}


static u32 get_probe_resp_offloads(u32 protocols)
{
	u32 ret = 0;

	if (protocols & NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS)
		ret |= DRIVER_CAPA_PROBE_RESP_OFFLOAD_WPS;
	if (protocols & NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS2)
		ret |= DRIVER_CAPA_PROBE_RESP_OFFLOAD_WPS2;
	if (protocols & NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P)
		ret |= DRIVER_CAPA_PROBE_RESP_OFFLOAD_P2P;
	if (protocols & NL80211_PROBE_RESP_OFFLOAD_SUPPORT_80211U)
		ret |= DRIVER_CAPA_PROBE_RESP_OFFLOAD_INTERWORKING;

	return ret;

}


static int capa_info_handler(struct nl_msg *msg, void *arg)
{
	struct nlattr *tb[NL80211_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct wfs_iface *cur_if = arg;
	struct nl80211_driver_info *info = cur_if->driver_info;

	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
			genlmsg_attrlen(gnlh, 0), NULL);

	if (tb[NL80211_ATTR_WIPHY_NAME])
		strncpy(info->wiphy_name,
			nla_get_string(tb[NL80211_ATTR_WIPHY_NAME]),
			MAX_WIPHY_NAME_LEN);

	if (tb[NL80211_ATTR_MAX_NUM_SCAN_SSIDS])
		info->capa.max_scan_ssids =
			nla_get_u8(tb[NL80211_ATTR_MAX_NUM_SCAN_SSIDS]);

	if (tb[NL80211_ATTR_MAX_NUM_SCHED_SCAN_SSIDS])
		info->capa.max_sched_scan_ssids =
			nla_get_u8(tb[NL80211_ATTR_MAX_NUM_SCHED_SCAN_SSIDS]);

	if (tb[NL80211_ATTR_MAX_MATCH_SETS])
		info->capa.max_match_sets =
			nla_get_u8(tb[NL80211_ATTR_MAX_MATCH_SETS]);

	if (tb[NL80211_ATTR_MAC_ACL_MAX])
		info->capa.max_acl_mac_addrs =
			nla_get_u8(tb[NL80211_ATTR_MAC_ACL_MAX]);

	capa_info_supported_iftypes(&info->capa,
			tb[NL80211_ATTR_SUPPORTED_IFTYPES]);
	capa_info_iface_comb(&info->capa,
			tb[NL80211_ATTR_INTERFACE_COMBINATIONS]);
	capa_info_supported_cmds(&info->capa,
			tb[NL80211_ATTR_SUPPORTED_COMMANDS]);
	capa_info_cipher_suites(&info->capa, tb[NL80211_ATTR_CIPHER_SUITES]);

	if (tb[NL80211_ATTR_OFFCHANNEL_TX_OK]) {
		wfs_printf(LOG_DEBUG, "Using driver-based off-channel TX");
		info->capa.flags |= DRIVER_CAPA_FLAGS_OFFCHANNEL_TX;
	}

	if (tb[NL80211_ATTR_ROAM_SUPPORT]) {
		wfs_printf(LOG_DEBUG, "Using driver-based roaming");
		info->capa.flags |= DRIVER_CAPA_FLAGS_BSS_SELECTION;
	}

	if (tb[NL80211_ATTR_MAX_REMAIN_ON_CHANNEL_DURATION])
		info->capa.max_roc_dur =
		nla_get_u32(tb[NL80211_ATTR_MAX_REMAIN_ON_CHANNEL_DURATION]);

	if (tb[NL80211_ATTR_SUPPORT_AP_UAPSD])
		info->capa.flags |= DRIVER_CAPA_FLAGS_AP_UAPSD;

	if (tb[NL80211_ATTR_TDLS_SUPPORT]) {
		wfs_printf(LOG_DEBUG, "TDLS supported");
		info->capa.flags |= DRIVER_CAPA_FLAGS_TDLS_SUPPORT;
	}

	if (tb[NL80211_ATTR_TDLS_EXTERNAL_SETUP]) {
		wfs_printf(LOG_DEBUG, "TDLS external setup");
		info->capa.flags |= DRIVER_CAPA_FLAGS_TDLS_EXTERNAL_SETUP;
	}

	if (tb[NL80211_ATTR_DEVICE_AP_SME])
		info->capa.device_ap_sme = 1;

	capa_info_feature_flags(&info->capa, tb[NL80211_ATTR_FEATURE_FLAGS]);

	if (tb[NL80211_ATTR_PROBE_RESP_OFFLOAD]) {
		wfs_printf(LOG_DEBUG, "Probe Response offload in AP mode "
				"supported");
		u32 protocols =
			nla_get_u32(tb[NL80211_ATTR_PROBE_RESP_OFFLOAD]);
		info->capa.flags |= DRIVER_CAPA_FLAGS_PROBE_RESP_OFFLOAD;
		info->capa.probe_resp_offloads =
				get_probe_resp_offloads(protocols);
	}

	if (tb[NL80211_ATTR_EXT_CAPA] && tb[NL80211_ATTR_EXT_CAPA_MASK]
			&& info->capa.ext_capa == NULL) {
		struct nlattr *ext = tb[NL80211_ATTR_EXT_CAPA];
		struct nlattr *ext_mask = tb[NL80211_ATTR_EXT_CAPA_MASK];
		u32 len = nla_len(ext);
		info->capa.ext_capa = (u8 *) zalloc(len);
		if (info->capa.ext_capa) {
			memcpy(info->capa.ext_capa, nla_data(ext), len);
			info->capa.ext_capa_len = len;
		}
		info->capa.ext_capa_mask = (u8 *) zalloc(len);
		if (info->capa.ext_capa_mask) {
			memcpy(info->capa.ext_capa_mask, nla_data(ext_mask),
					len);
		} else {
			FREE(info->capa.ext_capa);
			info->capa.ext_capa_len = 0;
		}
	}

	return NL_SKIP;
}


static enum wfs_internal_err
nl_get_capa_info(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	u32 if_idx = 0, feat = 0;
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	int err = 0;
	s32 flags = 0;

	assert(cur_if);

	ENTER();

	if_idx = if_nametoindex(cur_if->ifname);
	info = cur_if->driver_info;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Get driver capability info (if_idx=%d)", if_idx);

	ret = nl_get_proto_features(cur_if, &feat);

	flags = (feat & NL80211_PROTOCOL_FEATURE_SPLIT_WIPHY_DUMP) ?
			NLM_F_DUMP : 0;
	add_genl_header(msg, info->family_id, flags, NL80211_CMD_GET_WIPHY);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	NLA_PUT_FLAG(msg, NL80211_ATTR_SPLIT_WIPHY_DUMP);

	err = send_and_recv(cur_if, msg, capa_info_handler, cur_if);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to get driver capability info: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	if (!info->capa.connect) {
		wfs_printf(LOG_ERR, "Driver does not support "
			"authentication/association or connect commands");
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	if (!info->capa.max_roc_dur)
		info->capa.max_roc_dur = DRIVER_CAPA_DEFAULT_MAX_ROC_DURATION;

	wfs_printf(LOG_DEBUG, "Got driver capability info successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl_fill_capa(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;

	assert(cur_if);

	ENTER();

	ret = nl_get_capa_info(cur_if);
	if (ret)
		goto out;

	info = cur_if->driver_info;

	info->capa.filled = 1;

	info->capa.key_mgmt = DRIVER_CAPA_KEY_MGMT_WPA
			| DRIVER_CAPA_KEY_MGMT_WPA2
			| DRIVER_CAPA_KEY_MGMT_WPA_PSK
			| DRIVER_CAPA_KEY_MGMT_WPA2_PSK;

	info->capa.auth = DRIVER_CAPA_AUTH_OPEN
			| DRIVER_CAPA_AUTH_SHARED
			| DRIVER_CAPA_AUTH_LEAP;

	info->capa.flags |= DRIVER_CAPA_FLAGS_SANE_ERROR_CODES
			| DRIVER_CAPA_FLAGS_SET_KEYS_AFTER_ASSOC_DONE
			| DRIVER_CAPA_FLAGS_EAPOL_TX_STATUS;

	if (!info->capa.device_ap_sme)
		/**
		 * No AP SME is currently assumed to also indicate no AP MLME
		 * in the driver/firmware.
		 */
		info->capa.flags |= DRIVER_CAPA_FLAGS_DEAUTH_TX_STATUS
			| DRIVER_CAPA_FLAGS_AP_MLME;

	/**
	 * If probe client and socket tx status are supported, mac80211 is new
	 * enough to have everything we need to not need monitor interfaces.
	 */
	info->need_monitor = !info->capa.probe_client
				|| !info->capa.sk_tx_status;

	if (info->capa.device_ap_sme && info->need_monitor) {
		/**
		 * Non-mac80211 drivers may not support monitor interface.
		 * Make sure we do not get stuck with incorrect capability here
		 * by explicitly testing this.
		 */
		if (!info->capa.monitor) {
			wfs_printf(LOG_DEBUG, "Disable need_monitor with "
					"device_ap_sme since no monitor mode "
					"support detected");
			info->need_monitor = 0;
		}
	}

	/**
	 * If we aren't going to use monitor interfaces, but the
	 * driver doesn't support socket TX status, we won't get TX
	 * status for EAPOL frames.
	 */
	if (!info->need_monitor && !info->capa.sk_tx_status)
		info->capa.flags &= ~DRIVER_CAPA_FLAGS_EAPOL_TX_STATUS;

out:
	LEAVE();
	return ret;
}


static void eapol_tx_recv(int sock, void *ctx)
{
	struct wfs_iface *cur_if = ctx;
	// TODO: populate EAPOL TX receive handler
}


static int nl_mcast_group_handler(struct nl_msg *msg, void *arg)
{
	struct nl80211_mcast_group_info *mg_info = arg;
	struct nlattr *tb[CTRL_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nlattr *mcgrp = NULL;
	int rem = 0;

	nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
			genlmsg_attrlen(gnlh, 0), NULL);

	if (!tb[CTRL_ATTR_MCAST_GROUPS])
		return NL_SKIP;

	nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem) {
		struct nlattr *tb2[CTRL_ATTR_MCAST_GRP_MAX + 1];
		nla_parse(tb2, CTRL_ATTR_MCAST_GRP_MAX, nla_data(mcgrp),
				nla_len(mcgrp), NULL);
		if (!tb2[CTRL_ATTR_MCAST_GRP_NAME]
			|| !tb2[CTRL_ATTR_MCAST_GRP_ID]
			|| strncmp(nla_data(tb2[CTRL_ATTR_MCAST_GRP_NAME]),
				mg_info->group,
				nla_len(tb2[CTRL_ATTR_MCAST_GRP_NAME])) != 0)
			continue;
		mg_info->id = nla_get_u32(tb2[CTRL_ATTR_MCAST_GRP_ID]);
		break;
	}

	return NL_SKIP;
}


static int nl_get_mcast_group_id(struct wfs_iface *cur_if, const char *family,
		const char *group)
{
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	struct nl80211_mcast_group_info mg_info = { group, -ENOENT };
	int err = 0, ret = -ENOENT;

	assert(cur_if && cur_if->driver_info && family && group);

	ENTER();

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	info = cur_if->driver_info;

	genlmsg_put(msg, 0, 0, genl_ctrl_resolve(info->cmd_handle, "nlctrl"), 0,
			0, CTRL_CMD_GETFAMILY, 0);
	NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, family);

	err = send_and_recv(cur_if, msg, nl_mcast_group_handler, &mg_info);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to get multicast group ID: "
				"err=%d (%s)", err, strerror(-err));
		goto nla_put_failure;
	}

	ret = mg_info.id;

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl_add_mcast_membership(struct wfs_iface *cur_if, const char *family,
		const char *group)
{
	struct nl80211_driver_info *info = NULL;
	int mcast_group_id = 0, err = 0;

	assert(cur_if && cur_if->driver_info && family && group);

	info = cur_if->driver_info;

	mcast_group_id = nl_get_mcast_group_id(cur_if, family, group);
	if (mcast_group_id < 0)
		return WFS_INTERNAL_ERR_OS;

	err = nl_socket_add_membership(info->evt_handle, mcast_group_id);
	if (err)
		return WFS_INTERNAL_ERR_OS;

	return WFS_INTERNAL_ERR_SUCCESS;
}


/***********************************************************
 * Driver ops
 ***********************************************************
 */
static enum wfs_internal_err
nl80211_associate(struct wfs_driver_assoc_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct wfs_iface *cur_if = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(params);

	ENTER();

	cur_if = params->iface;
	info = cur_if->driver_info;

	if (!info)
		goto nla_put_failure;

	nl_mark_disconnected(cur_if);

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	wfs_printf(LOG_DEBUG, "Associate (if_idx=%d)", if_idx);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_ASSOCIATE);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	ret = nl_assoc_common(msg, params);
	if (ret)
		goto nla_put_failure;

	if (!is_zero_mac_addr(params->prev_bssid)) {
		wfs_printf(LOG_DEBUG, "prev_bssid=" MACSTR,
				MAC2STR(params->prev_bssid));
		NLA_PUT(msg, NL80211_ATTR_PREV_BSSID, ETH_ALEN,
				params->prev_bssid);
	}

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to request association: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send association request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_set_key(struct wfs_driver_key_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct wfs_iface *cur_if = NULL;
	u32 if_idx = 0;
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	int err = 0;
	int tdls = 0;

	assert(params);

	ENTER();

	cur_if = params->iface;
	if_idx = if_nametoindex(cur_if->ifname);
	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	/* Ignore for P2P Device */
	if (info->cur_status.nl_iftype == NL80211_IFTYPE_P2P_DEVICE)
		goto nla_put_failure;

	wfs_printf(LOG_DEBUG, "if_idx=%u cipher=%d key_idx=%d key_len=%u "
			"seq_len=%u set_tx=%d peer_addr=" MACSTR,
			if_idx, params->cipher, params->key_idx,
			params->key_len, params->seq_len, params->set_tx,
			MAC2STR(params->peer_addr));

#ifdef CONFIG_TDLS
	if (params->key_idx == -1) {
		params->key_idx = 0;
		tdls = 1;
	}
#endif

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if (!params->cipher) {
		add_genl_header(msg, info->family_id, 0, NL80211_CMD_DEL_KEY);
	} else {
		int cipher = 0;
		add_genl_header(msg, info->family_id, 0, NL80211_CMD_NEW_KEY);
		NLA_PUT(msg, NL80211_ATTR_KEY_DATA, params->key_len,
				params->key);
		cipher = get_cipher_suite(params->cipher);
		if (cipher == 0) {
			ret = WFS_INTERNAL_ERR_INVAL;
			goto nla_put_failure;
		}
		wfs_printf(LOG_DEBUG, "WLAN_CIPHER_SUITE: 0x%08X", cipher);
		NLA_PUT_U32(msg, NL80211_ATTR_KEY_CIPHER, cipher);
	}

	if (params->seq && params->seq_len)
		NLA_PUT(msg, NL80211_ATTR_KEY_SEQ, params->seq_len,
				params->seq);

	if (is_broadcast_mac_addr(params->peer_addr)) {
		struct nlattr *types;
		wfs_printf(LOG_DEBUG, "broadcast key");
		types = nla_nest_start(msg, NL80211_ATTR_KEY_DEFAULT_TYPES);
		if (!types)
			goto nla_put_failure;
		NLA_PUT_FLAG(msg, NL80211_KEY_DEFAULT_TYPE_MULTICAST);
		nla_nest_end(msg, types);
	} else if (!is_zero_mac_addr(params->peer_addr)) {
		wfs_printf(LOG_DEBUG, "addr=" MACSTR,
				MAC2STR(params->peer_addr));
		NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, params->peer_addr);
		if (params->cipher != CIPHER_WEP40
				&& params->cipher != CIPHER_WEP104
				&& params->key_idx && !params->set_tx) {
			wfs_printf(LOG_DEBUG, "RSN IBSS RX GTK");
			NLA_PUT_U32(msg, NL80211_ATTR_KEY_TYPE,
					NL80211_KEYTYPE_GROUP);
		}
	}

	NLA_PUT_U8(msg, NL80211_ATTR_KEY_IDX, params->key_idx);
	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if ((err == -ENOENT || err == -ENOLINK)
			&& !params->cipher)
		err = 0;
	if (err) {
		wfs_printf(LOG_ERR, "set_key failed: err=%d (%s)", err,
				strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	if (!params->set_tx || !params->cipher || tdls)
		goto done;
	if (is_ap_iftype(info->cur_status.nl_iftype)
			&& !is_zero_mac_addr(params->peer_addr)
			&& !is_broadcast_mac_addr(params->peer_addr))
		goto done;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_SET_KEY);
	NLA_PUT_U8(msg, NL80211_ATTR_KEY_IDX, params->key_idx);
	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	if (params->cipher == CIPHER_IGTK)
		NLA_PUT_FLAG(msg, NL80211_ATTR_KEY_DEFAULT_MGMT);
	else
		NLA_PUT_FLAG(msg, NL80211_ATTR_KEY_DEFAULT);

	if (!is_zero_mac_addr(params->peer_addr)) {
		struct nlattr *types =
			nla_nest_start(msg, NL80211_ATTR_KEY_DEFAULT_TYPES);
		if (!types)
			goto nla_put_failure;
		NLA_PUT_FLAG(msg, is_broadcast_mac_addr(params->peer_addr)
				? NL80211_KEY_DEFAULT_TYPE_MULTICAST
				: NL80211_KEY_DEFAULT_TYPE_UNICAST);
		nla_nest_end(msg, types);
	}

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err == -ENOENT)
		err = 0;
	if (err) {
		wfs_printf(LOG_ERR, "set_key failed: err=%d (%s)", err,
				strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

done:
	wfs_printf(LOG_DEBUG, "Send set_key request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_deauth(struct wfs_iface *cur_if, const u8 *addr, u16 reason_code)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;

	assert(cur_if && addr);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto done;

	nl_mark_disconnected(cur_if);

	if (!(info->capa.flags & DRIVER_CAPA_FLAGS_SME)) {
		ret = nl_disconnect(cur_if, reason_code);
		goto done;
	}
	wfs_printf(LOG_DEBUG, "%s(addr=" MACSTR " reason_code=%d)", __func__,
			MAC2STR(addr), reason_code);
	ret = nl_send_mlme_with_reason_code(cur_if, addr,
			NL80211_CMD_DEAUTHENTICATE, reason_code, 0);

done:
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_auth(struct wfs_driver_auth_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct wfs_iface *cur_if = NULL;
	struct nl_msg *msg = NULL;
	enum nl80211_auth_type type = NL80211_AUTHTYPE_OPEN_SYSTEM;
	u32 if_idx = 0;
	int err = 0;
	int is_retry = 0;
	int i = 0;
	int count = 0;

	assert(params);

	ENTER();

	cur_if = params->iface;
	info = cur_if->driver_info;

	if (!info)
		goto nla_put_failure;

	is_retry = info->cur_status.sta.retry_auth;
	/** Status update */
	nl_mark_disconnected(cur_if);
	info->cur_status.sta.retry_auth = 0;
	memset(info->cur_status.sta.auth_bssid, 0, ETH_ALEN);
	memcpy(info->cur_status.sta.auth_attempt_bssid, params->bssid,
			ETH_ALEN);

retry:
	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);
	wfs_printf(LOG_DEBUG, "Authenticate (if_idx=%d)", if_idx);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_AUTHENTICATE);

	for (i = 0; i < 4; ++i) {
		struct wfs_driver_key_params key_params;
		memset(&key_params, 0, sizeof(key_params));
		if (!params->wep_key[i])
			continue;
		key_params.cipher = (params->wep_key_len[i] == 5)
			? CIPHER_WEP40 : CIPHER_WEP104;
		key_params.key_idx = i;
		key_params.set_tx = (i == params->wep_tx_keyidx);
		key_params.key = params->wep_key[i];
		key_params.key_len = params->wep_key_len[i];
		nl80211_set_key(&key_params);
		if (!key_params.set_tx)
			continue;
		if (nl_add_key(msg, 1, &key_params)) {
			ret = WFS_INTERNAL_ERR_OS;
			goto nla_put_failure;
		}
	}

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	if (!is_zero_mac_addr(params->bssid)) {
		wfs_printf(LOG_DEBUG, "  * bssid=" MACSTR,
				MAC2STR(params->bssid));
		NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, params->bssid);
	}

	if (params->freq) {
		wfs_printf(LOG_DEBUG, "  * freq=%d", params->freq);
		NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, params->freq);
	}

	if (params->ssid) {
		wfs_hexdump_ascii(LOG_DEBUG, "  * SSID", params->ssid,
				params->ssid_len);
		if (params->ssid_len > MAX_SSID_LEN) {
			wfs_printf(LOG_ERR, "Invalid length of SSID: %lu",
					params->ssid_len);
			ret = WFS_INTERNAL_ERR_INVAL;
			goto nla_put_failure;
		}
		NLA_PUT(msg, NL80211_ATTR_SSID, params->ssid_len, params->ssid);
	}

	wfs_hexdump(LOG_DEBUG, "  * IEs", params->ie, params->ie_len);
	if (params->ie)
		NLA_PUT(msg, NL80211_ATTR_IE, params->ie_len, params->ie);

	if (params->sae_data) {
		wfs_hexdump(LOG_DEBUG, "  * SAE data", params->sae_data,
			    params->sae_data_len);
		NLA_PUT(msg, NL80211_ATTR_SAE_DATA, params->sae_data_len,
			params->sae_data);
	}

	if (params->auth_alg & AUTH_ALG_OPEN) {
		type = NL80211_AUTHTYPE_OPEN_SYSTEM;
	} else if (params->auth_alg & AUTH_ALG_SHARED) {
		type = NL80211_AUTHTYPE_SHARED_KEY;
	} else if (params->auth_alg & AUTH_ALG_LEAP) {
		type = NL80211_AUTHTYPE_NETWORK_EAP;
	} else if (params->auth_alg & AUTH_ALG_FT) {
		type = NL80211_AUTHTYPE_FT;
	} else if (params->auth_alg & AUTH_ALG_SAE) {
		type = NL80211_AUTHTYPE_SAE;
	} else {
		wfs_printf(LOG_ERR, "Unsupported auth_alg: 0x%0X",
				params->auth_alg);
		ret = WFS_INTERNAL_ERR_INVAL;
		goto nla_put_failure;
	}
	wfs_printf(LOG_DEBUG, "  * Auth type %d", type);
	NLA_PUT_U32(msg, NL80211_ATTR_AUTH_TYPE, type);

	if (params->local_state_change) {
		wfs_printf(LOG_DEBUG, "  * Local state change only");
		NLA_PUT_FLAG(msg, NL80211_ATTR_LOCAL_STATE_CHANGE);
	}

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_event(cur_if, LOG_DEBUG,
				"Failed to request authentication: err=%d (%s)",
				err, strerror(-err));
		++count;
		if (err == -EALREADY && count == 1
				&& !is_zero_mac_addr(params->bssid)
				&& !params->local_state_change) {
			/**
			 * mac80211 does not currently accept new
			 * authentication if we are already authenticated. As a
			 * workaround, force deauthentication and try again.
			 */
			wfs_printf(LOG_DEBUG, "Retry authentication after "
					"forced deauthentication");
			nl80211_deauth(cur_if, params->bssid,
					WLAN_REASON_PREV_AUTH_NOT_VALID);
			nlmsg_free(msg);
			goto retry;
		} else if (err == -ENOENT && params->freq && !is_retry) {
			/**
			 * cfg80211 has likely expired the BSS entry even
			 * though it was previously available in our internal
			 * BSS table. To recover quickly, start a single
			 * channel scan on the specified channel.
			 */
			// TODO
		} else if (is_retry) {
			wfs_printf(LOG_DEBUG, "Authentication retry failed");
			ret = WFS_INTERNAL_ERR_OS;
			goto nla_put_failure;
		}

		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send authentication request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_cancel_remain_on_channel(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;

	if (!info)
		goto nla_put_failure;

	if (!info->cur_status.roc.is_pending) {
		wfs_printf(LOG_ERR, "No pending remain-on-channel to cancel");
		ret = WFS_INTERNAL_ERR_INVAL;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Cancel remain-on-channel with cookie 0x%llX",
	(long long unsigned int) info->cur_status.roc.cookie);

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);

	add_genl_header(msg, info->family_id, 0,
			NL80211_CMD_CANCEL_REMAIN_ON_CHANNEL);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);
	NLA_PUT_U64(msg, NL80211_ATTR_COOKIE,
			info->cur_status.roc.cookie);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to cancel remain-on-channel: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send cancel-remain-on-channel request "
			"successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static void nl_destroy_handles(struct nl_handle **handle)
{
	if (*handle == NULL)
		return;
	nl80211_handle_destroy(*handle);
	*handle = NULL;
}


static enum wfs_internal_err
nl80211_deinit(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;

	if (!info)
		/** Already de-initialized. Just exit */
		goto out;

	/** Clean necessary info */
	if (is_ap_iftype(info->cur_status.nl_iftype))
		nl_del_beacon(cur_if);

	/** Free top level members */
	nl_destroy_handles(&info->cmd_handle);
	ret = wfs_loop_unregister_read_sock(nl_socket_get_fd(info->evt_handle),
			event_receive, cur_if->driver_info);
	nl_destroy_handles(&info->evt_handle);
	nl_cb_put(info->nl_cb);
	info->nl_cb = NULL;
	CLOSE(info->ioctl_sock);

	/** Free cur_status members */
	if (!info->cur_status.is_iface_already_up) {
		ret = turn_iface(cur_if, 0);
		if (ret)
			wfs_printf(LOG_ERR, "Failed to turn %s down",
					cur_if->ifname);
	}
	free_hw_info(info->cur_status.hw.hw_info,
			info->cur_status.hw.hw_info_num);
	info->cur_status.hw.hw_info = NULL;
	FREE(info->cur_status.scan.match_devs);
	if (info->capa.sk_tx_status) {
		ret = wfs_loop_unregister_read_sock(
				info->cur_status.ap.eapol_tx_sock,
				eapol_tx_recv, cur_if);
		CLOSE(info->cur_status.ap.eapol_tx_sock);
	}

	/** Free capa members */
	FREE(info->capa.ext_capa);
	FREE(info->capa.ext_capa_mask);

	/** Free info members */
	CLOSE(info->ioctl_sock);

	/** Update cur_if */
	FREE(info);
	cur_if->driver_info = NULL;

out:
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_del_sta(struct wfs_iface *cur_if, const u8 *addr)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if && addr);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	if_idx = if_nametoindex(cur_if->ifname);

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_DEL_STATION);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);
	NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, addr);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to delete stattion " MACSTR
			"err=%d (%s)", MAC2STR(addr), err, strerror(-err));
		ret = (err == -ENOENT) ? WFS_INTERNAL_ERR_SUCCESS :
					WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send del_station request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_get_bssid(struct wfs_iface *cur_if, u8 *bssid)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;

	assert(cur_if && bssid);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto out;

	if (!is_ap_iftype(info->cur_status.nl_iftype)
			&& !info->cur_status.sta.is_associated) {
		wfs_printf(LOG_ERR, "Not associated yet. Thus no BSSID.");
		ret = WFS_INTERNAL_ERR_INVAL;
		goto out;
	}

	memcpy(bssid, info->cur_status.bssid, ETH_ALEN);

out:
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_get_mac_addr(struct wfs_iface *cur_if, u8 *addr)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;

	assert(cur_if && addr);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto out;

	memcpy(addr, info->addr, ETH_ALEN);

out:
	LEAVE();
	return ret;
}


static struct wfs_driver_scan_results *
nl80211_get_scan_results(struct wfs_iface *cur_if)
{
	struct wfs_driver_scan_results *results = NULL;
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	struct nl80211_bss_info bss_info;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	msg = nlmsg_alloc();
	if (!msg)
		goto nla_put_failure;

	if_idx = if_nametoindex(cur_if->ifname);
	memset(&bss_info, 0, sizeof(bss_info));

	results = (struct wfs_driver_scan_results *) zalloc(sizeof(*results));
	if (!results) {
		wfs_printf(LOG_ERR, "%s", err_to_str(WFS_INTERNAL_ERR_OOM));
		goto nla_put_failure;
	}

	add_genl_header(msg, info->family_id, NLM_F_DUMP, NL80211_CMD_GET_SCAN);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	bss_info.info = info;
	bss_info.scan_results = results;
	err = send_and_recv(cur_if, msg, bss_info_handler, &bss_info);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to get scan results: "
				"err=%d (%s)", err, strerror(-err));
		goto nla_put_failure;
	}
	wfs_printf(LOG_DEBUG, "Received scan results (%u BSSes)", results->num);

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return results;
}


static enum wfs_internal_err
nl80211_get_ssid(struct wfs_iface *cur_if, u8 *ssid, u32 *ssid_len)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;

	assert(cur_if && ssid && ssid_len);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto out;

	if (!is_ap_iftype(info->cur_status.nl_iftype)
			&& !info->cur_status.sta.is_associated) {
		wfs_printf(LOG_ERR, "Not associated yet. Thus no SSID.");
		ret = WFS_INTERNAL_ERR_INVAL;
		goto out;
	}

	memcpy(ssid, info->cur_status.ssid, info->cur_status.ssid_len);
	*ssid_len = info->cur_status.ssid_len;

out:
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_if_add(struct wfs_driver_if_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct wfs_iface *cur_if = NULL;
	struct nl80211_driver_info *info = NULL;
	enum nl80211_iftype nl_iftype;

	assert(params);

	ENTER();

	cur_if = params->iface;
	info = cur_if->driver_info;
	nl_iftype = get_mapping_iftype(params->type);

	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_if_remove(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;

	assert(cur_if);

	ENTER();

	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_init(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	enum nl80211_iftype iftype = NL80211_IFTYPE_STATION;

	assert(cur_if);

	ENTER();

	info = (struct nl80211_driver_info *) zalloc(sizeof(*info));
	if (!info) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto out;
	}

	cur_if->driver_info = info;

	info->nl_cb = nl_cb_alloc(NL_CB_DEFAULT);
	if (!info->nl_cb) {
		wfs_printf(LOG_ERR, "Failed to allocate netlink callbacks");
		ret = WFS_INTERNAL_ERR_OOM;
		goto out;
	}

	info->cmd_handle = create_handle(info->nl_cb, "cmd_handle");
	if (!info->cmd_handle) {
		wfs_printf(LOG_ERR, "Failed to create cmd_handle socket");
		ret = WFS_INTERNAL_ERR_OS;
		goto out;
	}

	info->family_id = genl_ctrl_resolve(info->cmd_handle, "nl80211");
	if (info->family_id < 0) {
		wfs_printf(LOG_ERR, "'nl80211' generic netlink not found");
		ret = WFS_INTERNAL_ERR_OS;
		goto out;
	}

	info->evt_handle = create_handle(info->nl_cb, "evt_handle");
	if (!info->evt_handle) {
		wfs_printf(LOG_ERR, "Failed to create evt_handle socket");
		ret = WFS_INTERNAL_ERR_OS;
		goto out;
	}

	ret = nl_add_mcast_membership(cur_if, "nl80211", "scan");
	if (ret) {
		wfs_printf(LOG_ERR, "Failed to add multicast membership for "
				"scan events.");
		goto out;
	}

	ret = nl_add_mcast_membership(cur_if, "nl80211", "mlme");
	if (ret) {
		wfs_printf(LOG_ERR, "Failed to add multicast membership for "
				"mlme events.");
		goto out;
	}

	nl_add_mcast_membership(cur_if, "nl80211", "regulatory");

	nl_add_mcast_membership(cur_if, "nl80211", "vendor");

	nl_cb_set(info->nl_cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM,
			no_seq_check, NULL);
	nl_cb_set(info->nl_cb, NL_CB_VALID, NL_CB_CUSTOM,
			nl_process_event, cur_if);

	nl_socket_set_nonblocking(info->evt_handle);
	ret = wfs_loop_register_read_sock(nl_socket_get_fd(info->evt_handle),
			event_receive, cur_if->driver_info);
	if (ret)
		goto out;

	info->ioctl_sock = socket(AF_INET, SOCK_DGRAM, 0);
	if (info->ioctl_sock < 0) {
		wfs_perror("[ioctl_sock] socket");
		ret = WFS_INTERNAL_ERR_OS;
		wfs_printf(LOG_ERR, "%s", err_to_str(ret));
		goto out;
	}

	if (linux_is_iface_up(info->ioctl_sock, cur_if->ifname))
		info->cur_status.is_iface_already_up = 1;

	/** Fill driver capability information */
	ret = nl_fill_capa(cur_if);
	if (ret)
		goto out;

	/** Set iftype */
	ret = nl_set_iftype(cur_if, iftype);
	if (ret)
		goto out;

	/** Turn interface up */
	ret = turn_iface(cur_if, 1);
	if (ret) {
		wfs_printf(LOG_ERR, "Failed to turn up interface %s",
				cur_if->ifname);
		goto out;
	}

	/** Get MAC address */
	ret = linux_get_hwaddr(info->ioctl_sock, cur_if->ifname, info->addr);
	if (ret) {
		wfs_printf(LOG_ERR, "Failed to get interface %s MAC address",
				cur_if->ifname);
		goto out;
	}

	/** Initialize EAPOL TX socket for AP */
	if (!is_ap_iftype(info->cur_status.nl_iftype))
		goto out;

	info->cur_status.ap.eapol_tx_sock = socket(AF_PACKET, SOCK_DGRAM, 0);
	if (info->cur_status.ap.eapol_tx_sock < 0) {
		wfs_perror("[eapol_tx_sock] socket");
		ret = WFS_INTERNAL_ERR_OS;
		wfs_printf(LOG_ERR, "%s", err_to_str(ret));
		goto out;
	}
	if (info->capa.sk_tx_status) {
		int enabled = 1;
		if (setsockopt(info->cur_status.ap.eapol_tx_sock, SOL_SOCKET,
				SO_WIFI_STATUS, &enabled,
				sizeof(enabled)) < 0) {
			wfs_perror("[eapol_tx_sock] setsockopt");
			info->capa.sk_tx_status = 0;
			if (!info->need_monitor)
				info->capa.flags &=
					~DRIVER_CAPA_FLAGS_EAPOL_TX_STATUS;
		} else {
			ret = wfs_loop_register_read_sock(
					info->cur_status.ap.eapol_tx_sock,
					eapol_tx_recv, cur_if);
		}
	}

out:
	if (ret)
		nl80211_deinit(cur_if);

	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl_send_null_frame(struct wfs_iface *cur_if, const u8 *addr, u8 is_qos)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct wfs_driver_frame_params params;
	struct {
		struct ieee80211_hdr hdr;
		u16 qos_ctl;
	} STRUCT_PACKED nulldata;
	u32 size = 0;

	ENTER();

	assert(cur_if && addr);

	info = cur_if->driver_info;
	memset(&nulldata, 0, sizeof(nulldata));

	if (is_qos) {
		nulldata.hdr.frame_control = IEEE80211_FC(WLAN_FC_TYPE_DATA,
						WLAN_FC_STYPE_QOS_NULL);
		size = sizeof(nulldata);
	} else {
		nulldata.hdr.frame_control = IEEE80211_FC(WLAN_FC_TYPE_DATA,
						WLAN_FC_STYPE_NULLFUNC);
		size = sizeof(struct ieee80211_hdr);
	}

	nulldata.hdr.frame_control |= host_to_le16(WLAN_FC_FROMDS);
	memcpy(nulldata.hdr.IEEE80211_DA_FROMDS, addr, ETH_ALEN);
	memcpy(nulldata.hdr.IEEE80211_BSSID_FROMDS, info->cur_status.bssid,
			ETH_ALEN);
	memcpy(nulldata.hdr.IEEE80211_SA_FROMDS, info->cur_status.bssid,
			ETH_ALEN);

	memset(&params, 0, sizeof(params));
	params.iface = cur_if;
	params.frame_type = WFS_DRV_FT_MLME;
	params.data = (u8 *) &nulldata;
	params.data_len = size;
	params.freq = info->cur_status.freq;
	ret = nl80211_send_frame(&params);

	LEAVE();
	return ret;
}


int nl80211_is_scan_ongoing(struct wfs_iface *cur_if)
{
	int ret = FALSE;
	struct nl80211_driver_info *info = NULL;
	int state = 0;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto out;

	state = info->cur_status.scan.scan_state;

	if (state == SCAN_REQUESTED || state == SCAN_STARTED)
		ret = TRUE;

out:
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_probe_client(struct wfs_iface *cur_if, const u8 *addr, u8 is_qos)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if && addr);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	if (!info->capa.probe_client) {
		ret = nl_send_null_frame(cur_if, addr, is_qos);
		goto nla_put_failure;
	}

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_PROBE_CLIENT);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);
	NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, addr);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to probe client: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send probe_client request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_remain_on_channel(struct wfs_iface *cur_if, u32 freq, u32 duration)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	u64 cookie = 0;
	int err = 0;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	if_idx = if_nametoindex(cur_if->ifname);

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Remain-on-channel (if_idx=%d) "
			"freq=%u duration=%u", if_idx, freq, duration);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_REMAIN_ON_CHANNEL);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);
	NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, freq);
	NLA_PUT_U32(msg, NL80211_ATTR_DURATION, duration);

	err = send_and_recv(cur_if, msg, cookie_handler, &cookie);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to request remain-on-channel: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send remain-on-channel request successfully");
	wfs_printf(LOG_DEBUG, "Remain-on-channel cookie 0x%llX",
			(long long unsigned int) cookie);
	info->cur_status.roc.cookie = cookie;
	info->cur_status.roc.is_pending = 1;

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_scan(struct wfs_driver_scan_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct wfs_iface *cur_if = NULL;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	int err = 0;

	assert(params);

	ENTER();

	wfs_printf(LOG_DEBUG, "Scan request");

	cur_if = params->iface;
	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	msg = nl_build_scan_msg(cur_if, NL80211_CMD_TRIGGER_SCAN, params);
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	if (params->is_p2p_scan) {
		struct nlattr *rates =
			nla_nest_start(msg, NL80211_ATTR_SCAN_SUPP_RATES);
		wfs_printf(LOG_DEBUG, "P2P probe - mask SuppRates");
		if (!rates)
			goto nla_put_failure;
		/*
		 * Remove 2.4 GHz rates 1, 2, 5.5, 11 Mbps from supported rates
		 * by masking out everything else apart from the OFDM rates 6,
		 * 9, 12, 18, 24, 36, 48, 54 Mbps from non-MCS rates. All 5 GHz
		 * rates are left enabled.
		 */
		NLA_PUT(msg, NL80211_BAND_2GHZ, 8,
			"\x0c\x12\x18\x24\x30\x48\x60\x6c");
		nla_nest_end(msg, rates);
		NLA_PUT_FLAG(msg, NL80211_ATTR_TX_NO_CCK_RATE);
	}

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to trigger scan: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	info->cur_status.scan.scan_state = SCAN_REQUESTED;
	wfs_printf(LOG_DEBUG, "Send scan request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_sched_scan(struct wfs_driver_scan_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct wfs_iface *cur_if = NULL;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	int err = 0;
	int i = 0;
	u32 match_devs_num = 0;

	assert(params);

	ENTER();

	wfs_printf(LOG_DEBUG, "Schedule scan request");

	cur_if = params->iface;
	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	msg = nl_build_scan_msg(cur_if, NL80211_CMD_START_SCHED_SCAN, params);
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	NLA_PUT_U32(msg, NL80211_ATTR_SCHED_SCAN_INTERVAL, params->interval);

	match_devs_num = info->cur_status.scan.match_devs_num;
	if ((match_devs_num && match_devs_num <= info->capa.max_match_sets)
			|| params->rssi) {
		struct nlattr *match_sets =
			nla_nest_start(msg, NL80211_ATTR_SCHED_SCAN_MATCH);
		if (!match_sets) {
			ret = WFS_INTERNAL_ERR_OS;
			goto nla_put_failure;
		}

		for (i = 0; i < match_devs_num; ++i) {
			struct nlattr *match_ssid =
				nla_nest_start(msg, i + 1);
			wfs_hexdump_ascii(LOG_DEBUG, "Sched-scan match device",
				info->cur_status.scan.match_devs[i].ssid,
				info->cur_status.scan.match_devs[i].ssid_len);
			if (!match_ssid) {
				ret = WFS_INTERNAL_ERR_OS;
				goto nla_put_failure;
			}

			NLA_PUT(msg, NL80211_ATTR_SCHED_SCAN_MATCH_SSID,
				info->cur_status.scan.match_devs[i].ssid_len,
				info->cur_status.scan.match_devs[i].ssid);

			nla_nest_end(msg, match_ssid);
		}

		if (params->rssi) {
			struct nlattr *match_rssi = nla_nest_start(msg, 0);
			if (!match_rssi) {
				ret = WFS_INTERNAL_ERR_OS;
				goto nla_put_failure;
			}
			NLA_PUT_U32(msg, NL80211_SCHED_SCAN_MATCH_ATTR_RSSI,
					params->rssi);
			wfs_printf(LOG_DEBUG, "Sched-scan RSSI filter %d dBm",
					params->rssi);
			nla_nest_end(msg, match_rssi);
		}

		nla_nest_end(msg, match_sets);
	}

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to sched-scan: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	info->cur_status.scan.scan_state = SCHED_SCAN_REQUESTED;
	wfs_printf(LOG_DEBUG, "Send sched-scan request successfully with "
			"interval %d msecs", params->interval);

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_send_frame(struct wfs_driver_frame_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct wfs_iface *cur_if = NULL;
	struct nl80211_driver_info *info = NULL;
	u64 cookie = 0;
	u16 fc = 0;

	assert(params);

	ENTER();

	wfs_printf(LOG_DEBUG, "Send %s frame",
			frame_type_to_str(params->frame_type));

	cur_if = params->iface;
	info = cur_if->driver_info;
	if (!info)
		goto out;

	if (params->freq == 0) {
		params->freq = info->cur_status.freq;
		if (is_ap_iftype(info->cur_status.nl_iftype))
			wfs_printf(LOG_DEBUG, "Use BSS freq %u",
					params->freq);
		else
			wfs_printf(LOG_DEBUG, "Use associated freq %u",
					params->freq);
	}

	if (!params->is_header_needed) {
		struct ieee80211_mgmt *mgmt =
				(struct ieee80211_mgmt *) params->data;
		fc = le_to_host16(mgmt->frame_control);
		if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT
			&& WLAN_FC_GET_STYPE(fc) == WLAN_FC_STYPE_AUTH) {
			/**
			 * Only one of the authentication frame types is
			 * encrypted. In order for static WEP encryption to
			 * work properly (i.e., to not encrypt the frame), we
			 * need to tell mac80211 about the frames that must not
			 * be encrypted.
			 */
			u16 auth_alg = le_to_host16(mgmt->u.auth.auth_alg);
			u16 auth_trans =
				le_to_host16(mgmt->u.auth.auth_transaction);
			if (auth_alg != WLAN_AUTH_SHARED_KEY || auth_trans != 3)
				params->is_encrypted = 0;
		}
	}

	ret = nl_send_frame_core(params, &cookie);

	if (ret == WFS_INTERNAL_ERR_SUCCESS && !params->no_wait_ack
			&& params->frame_type == WFS_DRV_FT_ACTION) {
		wfs_printf(LOG_DEBUG,
			"Update frame cookie from 0x%llx to 0x%llx",
			(long long unsigned int) info->cur_status.frame_cookie,
			(long long unsigned int) cookie);
		info->cur_status.frame_cookie = cookie;
	}

out:
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_send_frame_wait_cancel(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if);

	ENTER();

	if_idx = if_nametoindex(cur_if->ifname);
	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	wfs_printf(LOG_DEBUG, "Cancel TX frame wait: cookie=0x%llx",
		(long long unsigned int) info->cur_status.frame_cookie);

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_FRAME_WAIT_CANCEL);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	NLA_PUT_U64(msg, NL80211_ATTR_COOKIE, info->cur_status.frame_cookie);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to cancel frame wait: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send frame-wait-cancel request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_get_country(struct wfs_iface *cur_if, u8 *country_code)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	u32 if_idx = 0;
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	int err = 0;

	assert(cur_if && country_code);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	if_idx = if_nametoindex(cur_if->ifname);

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_GET_REG);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	country_code[0] = '\0';
	err = send_and_recv(cur_if, msg, get_country_handler, country_code);
	msg = NULL;
	if (err || !country_code[0]) {
		wfs_printf(LOG_ERR, "Failed to get-country: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Get country: %s", country_code);

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_get_hw_info(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	u32 if_idx = 0, feat = 0;
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	int err = 0;
	s32 flags = 0;
	struct nl80211_wiphy_info wiphy_info;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	if_idx = if_nametoindex(cur_if->ifname);
	memset(&wiphy_info, 0, sizeof(wiphy_info));

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Get hardware info (if_idx=%d)", if_idx);

	ret = nl_get_proto_features(cur_if, &feat);

	flags = (feat & NL80211_PROTOCOL_FEATURE_SPLIT_WIPHY_DUMP) ?
			NLM_F_DUMP : 0;
	add_genl_header(msg, info->family_id, flags, NL80211_CMD_GET_WIPHY);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	NLA_PUT_FLAG(msg, NL80211_ATTR_SPLIT_WIPHY_DUMP);

	err = send_and_recv(cur_if, msg, wiphy_info_handler, &wiphy_info);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to get hardware info: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	ret = nl_set_regulatory_flags(cur_if, &wiphy_info);
	if (ret)
		goto nla_put_failure;

	ret = post_process_hw_info(cur_if, &wiphy_info);
	if (ret)
		goto nla_put_failure;

	wfs_printf(LOG_DEBUG, "Got hardware info successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_set_country(struct wfs_iface *cur_if, const u8 *country_code)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	u32 if_idx = 0;
	struct nl_msg *msg = NULL;
	struct nl80211_driver_info *info = NULL;
	char code[3];
	int err = 0;

	assert(cur_if && country_code);

	ENTER();

	memset(code, 0, sizeof(code));
	memcpy(code, country_code, sizeof(code) - 1);
	if_idx = if_nametoindex(cur_if->ifname);
	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_REQ_SET_REG);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	NLA_PUT_STRING(msg, NL80211_ATTR_REG_ALPHA2, code);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to set-country: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send set-country request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_set_cqm(struct wfs_driver_cqm_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct wfs_iface *cur_if = NULL;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	struct nlattr *cqm = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(params);

	ENTER();

	cur_if = params->iface;
	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	if_idx = if_nametoindex(cur_if->ifname);

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_SET_CQM);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	cqm = nla_nest_start(msg, NL80211_ATTR_CQM);
	if (cqm == NULL) {
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	NLA_PUT_U32(msg, NL80211_ATTR_CQM_RSSI_THOLD, params->threshold);
	NLA_PUT_U32(msg, NL80211_ATTR_CQM_RSSI_HYST, params->hysteresis);

	nla_nest_end(msg, cqm);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to set-cqm: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send set-cqm request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_set_mac_addr(struct wfs_iface *cur_if, const u8 *addr)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;

	assert(cur_if && addr);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto out;

	ret = linux_set_hwaddr(info->ioctl_sock, cur_if->ifname, addr);
	if (ret) {
		wfs_printf(LOG_ERR, "Failed to set MAC address: "MACSTR,
				MAC2STR(addr));
		goto out;
	}
	memcpy(info->addr, addr, ETH_ALEN);

out:
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_sta_add(struct wfs_driver_sta_add_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct wfs_iface *cur_if = NULL;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	char *action = NULL;
	int err = 0;

	assert(params);

	ENTER();

	cur_if = params->iface;
	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	if_idx = if_nametoindex(cur_if->ifname);

	action = (params->is_new) ? "Add" : "Update";
	wfs_printf(LOG_DEBUG, "%s STA " MACSTR, action, MAC2STR(params->addr));

	if ((params->sta_flags & WFS_DRV_STA_TDLS_PEER)
		&& !(info->capa.flags & DRIVER_CAPA_FLAGS_TDLS_SUPPORT)) {
		ret = WFS_INTERNAL_ERR_INVAL;
		goto nla_put_failure;
	}

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	add_genl_header(msg, info->family_id, 0, params->is_new ?
			NL80211_CMD_NEW_STATION : NL80211_CMD_SET_STATION);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, params->addr);
	wfs_hexdump(LOG_DEBUG, "supported rates", params->supp_rates,
			params->supp_rates_len);
	NLA_PUT(msg, NL80211_ATTR_STA_SUPPORTED_RATES, params->supp_rates_len,
			params->supp_rates);

	if (params->is_new) {
		if (params->aid) {
			wfs_printf(LOG_DEBUG, "aid=%u", params->aid);
			NLA_PUT_U16(msg, NL80211_ATTR_STA_AID, params->aid);
		} else {
			/**
			 * cfg80211 validates that AID is non-zero, so we have
			 * to make this a non-zero value for the TDLS case where
			 * a dummy STA entry is used for now.
			 */
			wfs_printf(LOG_DEBUG, "aid=1 (TDLS workaround)");
			NLA_PUT_U16(msg, NL80211_ATTR_STA_AID, 1);
		}
		wfs_printf(LOG_DEBUG, "listen_interval=%u",
				params->listen_interval);
		NLA_PUT_U16(msg, NL80211_ATTR_STA_LISTEN_INTERVAL,
				params->listen_interval);
	} else if ((params->sta_flags & WFS_DRV_STA_TDLS_PEER) && params->aid) {
		wfs_printf(LOG_DEBUG, "peer_aid=%u", params->aid);
		NLA_PUT_U16(msg, NL80211_ATTR_PEER_AID, params->aid);
	}

	if (params->ht_caps) {
		wfs_hexdump(LOG_DEBUG, "ht_capabilities",
			(u8 *) params->ht_caps, sizeof(*params->ht_caps));
		NLA_PUT(msg, NL80211_ATTR_HT_CAPABILITY,
				sizeof(*params->ht_caps), params->ht_caps);
	}

	if (params->vht_caps) {
		wfs_hexdump(LOG_DEBUG, "vht_capabilities",
			(u8 *) params->vht_caps, sizeof(*params->vht_caps));
		NLA_PUT(msg, NL80211_ATTR_VHT_CAPABILITY,
				sizeof(*params->vht_caps), params->vht_caps);
	}

	wfs_printf(LOG_DEBUG, "sta_capabilities=0x%x", params->sta_caps);
	NLA_PUT_U16(msg, NL80211_ATTR_STA_CAPABILITY, params->sta_caps);

	if (params->ext_caps && params->ext_caps_len) {
		wfs_hexdump(LOG_DEBUG, "ext_caps", params->ext_caps,
				params->ext_caps_len);
		NLA_PUT(msg, NL80211_ATTR_STA_EXT_CAPABILITY,
				params->ext_caps_len, params->ext_caps);
	}

	ret = nl_set_sta_flags2(msg, params->sta_flags, params->sta_flags);
	if (ret)
		goto nla_put_failure;

	if (params->sta_flags & WFS_DRV_STA_WME) {
		struct nlattr *wme = nla_nest_start(msg, NL80211_ATTR_STA_WME);
		if (!wme)
			goto nla_put_failure;
		wfs_printf(LOG_DEBUG, "qos_info=0x%x", params->qos_info);
		NLA_PUT_U8(msg, NL80211_STA_WME_UAPSD_QUEUES,
				params->qos_info & WMM_QOSINFO_STA_AC_MASK);
		NLA_PUT_U8(msg, NL80211_STA_WME_MAX_SP,
				(params->qos_info >> WMM_QOSINFO_STA_SP_SHIFT) &
				WMM_QOSINFO_STA_SP_MASK);
		nla_nest_end(msg, wme);
	}

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to %s station: err=%d (%s)",
				action, err, strerror(-err));
		ret = (err == -EEXIST) ? WFS_INTERNAL_ERR_SUCCESS :
					WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send %s station request successfully", action);

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_sta_deauth(struct wfs_iface *cur_if, const u8 *addr, u16 reason_code)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;

	assert(cur_if && addr);

	ENTER();

	ret = nl_sta_disconnect(cur_if, addr, reason_code,
			WLAN_FC_STYPE_DEAUTH);

	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_sta_disassoc(struct wfs_iface *cur_if, const u8 *addr, u16 reason_code)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;

	assert(cur_if && addr);

	ENTER();

	ret = nl_sta_disconnect(cur_if, addr, reason_code,
			WLAN_FC_STYPE_DISASSOC);

	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_set_ap(struct wfs_driver_ap_params *params)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct wfs_iface *cur_if = NULL;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	u8 cmd = NL80211_CMD_NEW_BEACON;
	int err = 0;
	u32 version = 0;
	u32 suites[10];
	int suite_num = 0;
	u32 group_cipher = 0;

	assert(params);

	ENTER();

	cur_if = params->iface;
	if_idx = if_nametoindex(cur_if->ifname);
	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	wfs_printf(LOG_DEBUG, "Set beacon (beacon_set=%d)",
			info->cur_status.ap.is_beacon_set);
	if (info->cur_status.ap.is_beacon_set)
		cmd = NL80211_CMD_SET_BEACON;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	add_genl_header(msg, info->family_id, 0, cmd);

	wfs_printf(LOG_DEBUG, "ifindex=%d", if_idx);
	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);
	wfs_hexdump(LOG_DEBUG, "Beacon head", params->head, params->head_len);
	NLA_PUT(msg, NL80211_ATTR_BEACON_HEAD, params->head_len, params->head);
	wfs_hexdump(LOG_DEBUG, "Beacon tail", params->tail, params->tail_len);
	NLA_PUT(msg, NL80211_ATTR_BEACON_TAIL, params->tail_len, params->tail);
	wfs_printf(LOG_DEBUG, "interval=%d", params->interval);
	NLA_PUT_U32(msg, NL80211_ATTR_BEACON_INTERVAL, params->interval);
	wfs_printf(LOG_DEBUG, "dtim_period=%d", params->dtim_period);
	NLA_PUT_U32(msg, NL80211_ATTR_DTIM_PERIOD, params->dtim_period);
	wfs_hexdump_ascii(LOG_DEBUG, "SSID", params->ssid, params->ssid_len);
	NLA_PUT(msg, NL80211_ATTR_SSID, params->ssid_len, params->ssid);
	if (params->probe_resp && params->probe_resp_len) {
		wfs_hexdump(LOG_DEBUG, "Probe resp", params->probe_resp,
				params->probe_resp_len);
		NLA_PUT(msg, NL80211_ATTR_PROBE_RESP, params->probe_resp_len,
			params->probe_resp);
	}
	switch (params->hidden_ssid) {
	case WFS_DRV_HIDDEN_SSID_NOT_IN_USE:
		wfs_printf(LOG_DEBUG, "hidden SSID not in use");
		NLA_PUT_U32(msg, NL80211_ATTR_HIDDEN_SSID,
				NL80211_HIDDEN_SSID_NOT_IN_USE);
		break;
	case WFS_DRV_HIDDEN_SSID_ZERO_LEN:
		wfs_printf(LOG_DEBUG, "hidden SSID zero len");
		NLA_PUT_U32(msg, NL80211_ATTR_HIDDEN_SSID,
				NL80211_HIDDEN_SSID_ZERO_LEN);
		break;
	case WFS_DRV_HIDDEN_SSID_ZERO_CONTENTS:
		wfs_printf(LOG_DEBUG, "hidden SSID zero contents");
		NLA_PUT_U32(msg, NL80211_ATTR_HIDDEN_SSID,
				NL80211_HIDDEN_SSID_ZERO_CONTENTS);
		break;
	}

	wfs_printf(LOG_DEBUG, "is_privacy=%d", params->is_privacy);
	if (params->is_privacy)
		NLA_PUT_FLAG(msg, NL80211_ATTR_PRIVACY);

	wfs_printf(LOG_DEBUG, "auth_alg=0x%x", params->auth_alg);
	if ((params->auth_alg & (AUTH_ALG_OPEN | AUTH_ALG_SHARED)) ==
			(AUTH_ALG_OPEN | AUTH_ALG_SHARED)) {
		/** Leave out the attribute */
	} else if (params->auth_alg & AUTH_ALG_OPEN) {
		NLA_PUT_U32(msg, NL80211_ATTR_AUTH_TYPE,
				NL80211_AUTHTYPE_OPEN_SYSTEM);
	} else if (params->auth_alg & AUTH_ALG_SHARED) {
		NLA_PUT_U32(msg, NL80211_ATTR_AUTH_TYPE,
				NL80211_AUTHTYPE_SHARED_KEY);
	}

	wfs_printf(LOG_DEBUG, "wpa_version=0x%x", params->wpa_version);
	if (params->wpa_version & SEC_PROTO_WPA)
		version |= NL80211_WPA_VERSION_1;
	if (params->wpa_version & SEC_PROTO_RSN)
		version |= NL80211_WPA_VERSION_2;
	if (version)
		NLA_PUT_U32(msg, NL80211_ATTR_WPA_VERSIONS, version);

	wfs_printf(LOG_DEBUG, "key_mgmt_suites=0x%x", params->key_mgmt_suites);
	if (params->key_mgmt_suites & KEY_MGMT_IEEE8021X)
		suites[suite_num++] = WLAN_AKM_SUITE_8021X;
	if (params->key_mgmt_suites & KEY_MGMT_PSK)
		suites[suite_num++] = WLAN_AKM_SUITE_PSK;
	if (suite_num)
		NLA_PUT(msg, NL80211_ATTR_AKM_SUITES, suite_num * sizeof(u32),
				suites);

	wfs_printf(LOG_DEBUG, "pairwise_ciphers=0x%x",
			params->pairwise_ciphers);
	suite_num = get_cipher_suites(params->pairwise_ciphers, suites,
			ARRAY_SIZE(suites));
	if (suite_num)
		NLA_PUT(msg, NL80211_ATTR_CIPHER_SUITES_PAIRWISE,
				suite_num * sizeof(u32), suites);

	wfs_printf(LOG_DEBUG, "group_cipher=0x%x",
			params->group_cipher);
	group_cipher = get_cipher_suite(params->group_cipher);
	if (group_cipher)
		NLA_PUT_U32(msg, NL80211_ATTR_CIPHER_SUITE_GROUP, group_cipher);

	if (params->beacon_ie && params->beacon_ie_len) {
		wfs_hexdump(LOG_DEBUG, "beacon_ie", params->beacon_ie,
				params->beacon_ie_len);
		NLA_PUT(msg, NL80211_ATTR_IE, params->beacon_ie_len,
				params->beacon_ie);
	}
	if (params->probe_resp_ie && params->probe_resp_ie_len) {
		wfs_hexdump(LOG_DEBUG, "probe_resp_ie", params->probe_resp_ie,
				params->probe_resp_ie_len);
		NLA_PUT(msg, NL80211_ATTR_IE, params->probe_resp_ie_len,
				params->probe_resp_ie);
	}
	if (params->assoc_resp_ie && params->assoc_resp_ie_len) {
		wfs_hexdump(LOG_DEBUG, "assoc_resp_ie", params->assoc_resp_ie,
				params->assoc_resp_ie_len);
		NLA_PUT(msg, NL80211_ATTR_IE, params->assoc_resp_ie_len,
				params->assoc_resp_ie);
	}

	if (info->capa.flags & DRIVER_CAPA_FLAGS_INACTIVITY_TIMER) {
		wfs_printf(LOG_DEBUG, "ap_max_inactivity=%d",
				params->ap_max_inactivity);
		NLA_PUT_U16(msg, NL80211_ATTR_INACTIVITY_TIMEOUT,
				params->ap_max_inactivity);
	}

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to set_ap: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	info->cur_status.ap.is_beacon_set = 1;
	ret = nl_set_bss(params);
	if (ret)
		goto nla_put_failure;

	wfs_printf(LOG_DEBUG, "Send set_ap request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_status(struct wfs_iface *cur_if, u8 *buf, u32 buf_len, u32 *filled_len)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_INVAL;
	struct nl80211_driver_info *info = NULL;
	char *pos = NULL, *end = NULL;
	int res = 0;

	ENTER();

	info = cur_if->driver_info;
	pos = (char *) buf;
	end = (char *) buf + buf_len;

	if (!info)
		goto done;

	/** Top level in struct nl80211_driver_info */
	res = snprintf(pos, end - pos,
			"family_id=%d\n"
			"addr="MACSTR"\n"
			"wiphy_name=%s\n"
			"%s",
			/** Top level in struct nl80211_driver_info */
			info->family_id,
			MAC2STR(info->addr),
			info->wiphy_name,
			info->need_monitor ? "need_monitor=1\n" : ""
			);
	if (res < 0 || res >= end - pos)
		goto done;
	pos += res;

	/** Capa level in struct nl80211_driver_info */
	if (info->capa.filled) {
		res = snprintf(pos, end - pos,
			"key_mgmt=0x%x\n"
			"enc=0x%x\n"
			"auth=0x%x\n"
			"flags=0x%x\n"
			"max_scan_ssids=%u\n"
			"max_sched_scan_ssids=%u\n"
			"max_match_sets=%u\n"
			"max_roc_dur=%u\n"
			"max_stations=%u\n"
			"probe_resp_offloads=0x%x\n"
			"max_acl_mac_addrs=%u\n"
			"num_concur_chans=%u\n"
			"%s%s%s%s%s%s",
			info->capa.key_mgmt,
			info->capa.enc,
			info->capa.auth,
			info->capa.flags,
			info->capa.max_scan_ssids,
			info->capa.max_sched_scan_ssids,
			info->capa.max_match_sets,
			info->capa.max_roc_dur,
			info->capa.max_stations,
			info->capa.probe_resp_offloads,
			info->capa.max_acl_mac_addrs,
			info->capa.num_concur_chans,
			info->capa.device_ap_sme ? "device_ap_sme=1\n" : "",
			info->capa.probe_client ? "probe_client=1\n" : "",
			info->capa.monitor ? "monitor=1\n" : "",
			info->capa.connect ? "connect=1\n" : "",
			info->capa.sched_scan ? "sched_scan=1\n" : "",
			info->capa.sk_tx_status ? "sk_tx_status=1\n" : ""
			);
		if (res < 0 || res >= end - pos)
			goto done;
		pos += res;
	}

	/** Cur_status level in struct nl80211_driver_info */
	res = snprintf(pos, end - pos,
			"nl_iftype=%s\n"
			"",
			iftype_to_str(info->cur_status.nl_iftype)
			);
	if (res < 0 || res >= end - pos)
		goto done;
	pos += res;

	ret = WFS_INTERNAL_ERR_SUCCESS;

done:
	*filled_len = pos - (char *) buf;

	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_stop_ap(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto done;

	if (!is_ap_iftype(info->cur_status.nl_iftype)) {
		ret = WFS_INTERNAL_ERR_NOTSUPP;
		goto done;
	}

	ret = nl_del_beacon(cur_if);
	if (ret)
		goto done;

	info->cur_status.ap.is_beacon_set = 0;

done:
	LEAVE();
	return ret;
}


static enum wfs_internal_err
nl80211_stop_sched_scan(struct wfs_iface *cur_if)
{
	enum wfs_internal_err ret = WFS_INTERNAL_ERR_SUCCESS;
	struct nl80211_driver_info *info = NULL;
	struct nl_msg *msg = NULL;
	u32 if_idx = 0;
	int err = 0;

	assert(cur_if);

	ENTER();

	info = cur_if->driver_info;
	if (!info)
		goto nla_put_failure;

	msg = nlmsg_alloc();
	if (!msg) {
		ret = WFS_INTERNAL_ERR_OOM;
		goto nla_put_failure;
	}

	if_idx = if_nametoindex(cur_if->ifname);

	add_genl_header(msg, info->family_id, 0, NL80211_CMD_STOP_SCHED_SCAN);

	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, if_idx);

	err = send_and_recv(cur_if, msg, NULL, NULL);
	msg = NULL;
	if (err) {
		wfs_printf(LOG_ERR, "Failed to stop scheduled scan: "
				"err=%d (%s)", err, strerror(-err));
		ret = WFS_INTERNAL_ERR_OS;
		goto nla_put_failure;
	}

	wfs_printf(LOG_DEBUG, "Send stop_sched_scan request successfully");

nla_put_failure:
	nlmsg_free(msg);
	LEAVE();
	return ret;
}


const struct wfs_driver_ops wfs_driver_nl80211_ops = {
	.name = "nl80211",
	.desc = "Linux nl80211/cfg80211",
	.associate = nl80211_associate,
	.auth = nl80211_auth,
	.cancel_remain_on_channel = nl80211_cancel_remain_on_channel,
	.deauth = nl80211_deauth,
	.deinit = nl80211_deinit,
	.del_sta = nl80211_del_sta,
	.get_bssid = nl80211_get_bssid,
	.get_country = nl80211_get_country,
	.get_hw_info = nl80211_get_hw_info,
	.get_mac_addr = nl80211_get_mac_addr,
	.get_scan_results = nl80211_get_scan_results,
	.get_ssid = nl80211_get_ssid,
	.if_add = nl80211_if_add,
	.if_remove = nl80211_if_remove,
	.init = nl80211_init,
	.is_scan_ongoing = nl80211_is_scan_ongoing,
	.probe_client = nl80211_probe_client,
	.remain_on_channel = nl80211_remain_on_channel,
	.scan = nl80211_scan,
	.sched_scan = nl80211_sched_scan,
	.send_frame = nl80211_send_frame,
	.send_frame_wait_cancel = nl80211_send_frame_wait_cancel,
	.set_ap = nl80211_set_ap,
	.set_country = nl80211_set_country,
	.set_cqm = nl80211_set_cqm,
	.set_key = nl80211_set_key,
	.set_mac_addr = nl80211_set_mac_addr,
	.sta_add = nl80211_sta_add,
	.sta_deauth = nl80211_sta_deauth,
	.sta_disassoc = nl80211_sta_disassoc,
	.status = nl80211_status,
	.stop_ap = nl80211_stop_ap,
	.stop_sched_scan = nl80211_stop_sched_scan,
};
